From ccee36bb689bcc6df75acf1ddcb72d006c4e7770 Mon Sep 17 00:00:00 2001
From: David Sidrane <David.Sidrane@NscDg.com>
Date: Mon, 15 Mar 2021 13:13:37 -0700
Subject: [PATCH] sd_bench and logger use aligned buffers

---
 .../common/include/px4_platform_common/posix.h  |  3 +++
 .../px4/common/include/px4_platform/micro_hal.h |  3 +++
 src/modules/logger/log_writer_file.cpp          |  2 +-
 src/systemcmds/sd_bench/sd_bench.c              | 17 +++++++++++++++--
 4 files changed, 22 insertions(+), 3 deletions(-)

diff --git a/platforms/common/include/px4_platform_common/posix.h b/platforms/common/include/px4_platform_common/posix.h
index fec109e86c..5e7a8c3c70 100644
--- a/platforms/common/include/px4_platform_common/posix.h
+++ b/platforms/common/include/px4_platform_common/posix.h
@@ -83,6 +83,9 @@ typedef pollevent_t px4_pollevent_t;
 
 #define	 PX4_STACK_OVERHEAD	(1024 * 24)
 
+#define px4_cache_aligned_data()
+#define px4_cache_aligned_alloc malloc
+
 __BEGIN_DECLS
 
 typedef short px4_pollevent_t;
diff --git a/platforms/posix/src/px4/common/include/px4_platform/micro_hal.h b/platforms/posix/src/px4/common/include/px4_platform/micro_hal.h
index 464626da76..5029e44246 100644
--- a/platforms/posix/src/px4/common/include/px4_platform/micro_hal.h
+++ b/platforms/posix/src/px4/common/include/px4_platform/micro_hal.h
@@ -40,3 +40,6 @@
 
 #define px4_udelay(usec) px4_usleep(usec)
 #define px4_mdelay(msec) px4_msleep(msec)
+
+#define px4_cache_aligned_data()
+#define px4_cache_aligned_alloc malloc
diff --git a/src/modules/logger/log_writer_file.cpp b/src/modules/logger/log_writer_file.cpp
index d6f52182a3..40e6ff45cd 100644
--- a/src/modules/logger/log_writer_file.cpp
+++ b/src/modules/logger/log_writer_file.cpp
@@ -457,7 +457,7 @@ bool LogWriterFile::LogFileBuffer::start_log(const char *filename)
 	}
 
 	if (_buffer == nullptr) {
-		_buffer = new uint8_t[_buffer_size];
+		_buffer = (uint8_t *) px4_cache_aligned_alloc(_buffer_size);
 
 		if (_buffer == nullptr) {
 			PX4_ERR("Can't create log buffer");
diff --git a/src/systemcmds/sd_bench/sd_bench.c b/src/systemcmds/sd_bench/sd_bench.c
index 93556847e0..6c0577e4b0 100644
--- a/src/systemcmds/sd_bench/sd_bench.c
+++ b/src/systemcmds/sd_bench/sd_bench.c
@@ -79,8 +79,10 @@ usage()
 	PRINT_MODULE_USAGE_PARAM_INT('r', 5, 1, 1000, "Number of runs", true);
 	PRINT_MODULE_USAGE_PARAM_INT('d', 2000, 1, 100000, "Duration of a run in ms", true);
 	PRINT_MODULE_USAGE_PARAM_FLAG('s', "Call fsync after each block (default=at end of each run)", true);
+	PRINT_MODULE_USAGE_PARAM_FLAG('u', "Test performance with unaligned data)", true);
 }
 
+
 int
 sd_bench_main(int argc, char *argv[])
 {
@@ -91,8 +93,10 @@ sd_bench_main(int argc, char *argv[])
 	synchronized = false;
 	num_runs = 5;
 	run_duration = 2000;
+	bool aligned = true;
+	uint8_t *block =  NULL;
 
-	while ((ch = px4_getopt(argc, argv, "b:r:d:s", &myoptind, &myoptarg)) != EOF) {
+	while ((ch = px4_getopt(argc, argv, "b:r:d:su", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
 		case 'b':
 			block_size = strtol(myoptarg, NULL, 0);
@@ -110,6 +114,10 @@ sd_bench_main(int argc, char *argv[])
 			synchronized = true;
 			break;
 
+		case 'u':
+			aligned = false;
+			break;
+
 		default:
 			usage();
 			return -1;
@@ -130,7 +138,12 @@ sd_bench_main(int argc, char *argv[])
 	}
 
 	//create some data block
-	uint8_t *block = (uint8_t *)malloc(block_size);
+	if (aligned) {
+		block = (uint8_t *)px4_cache_aligned_alloc(block_size);
+
+	} else {
+		block = (uint8_t *)malloc(block_size);
+	}
 
 	if (!block) {
 		PX4_ERR("Failed to allocate memory block");