From cb2329838411174e8cb35ee1c79f5e19bce5c956 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 13 Jan 2014 14:25:16 +1100
Subject: [PATCH] DataFlash: try to keep dataflash writes aligned

also rename lastlog.txt to LASTLOG.TXT as NuttX seems to get less
corruption with VFAT filenames that don't need uppercase mappings
---
 libraries/DataFlash/DataFlash_File.cpp | 41 +++++++++++++++++++++-----
 libraries/DataFlash/DataFlash_File.h   | 17 +++++++++++
 2 files changed, 51 insertions(+), 7 deletions(-)

diff --git a/libraries/DataFlash/DataFlash_File.cpp b/libraries/DataFlash/DataFlash_File.cpp
index ae18a128b9..ecae8808ad 100644
--- a/libraries/DataFlash/DataFlash_File.cpp
+++ b/libraries/DataFlash/DataFlash_File.cpp
@@ -35,13 +35,22 @@ extern const AP_HAL::HAL& hal;
 DataFlash_File::DataFlash_File(const char *log_directory) :
     _write_fd(-1),
     _read_fd(-1),
+    _read_offset(0),
+    _write_offset(0),
     _initialised(false),
     _log_directory(log_directory),
     _writebuf(NULL),
-    _writebuf_size(4096),
+    _writebuf_size(16*1024),
+    _writebuf_chunk(512), // until multiblock write support works on
+                          // px4, we are best off keeping writes small
     _writebuf_head(0),
     _writebuf_tail(0),
     _last_write_time(0)
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+    ,_perf_write(perf_alloc(PC_ELAPSED, "DF_write")),
+    _perf_fsync(perf_alloc(PC_ELAPSED, "DF_fsync")),
+    _perf_errors(perf_alloc(PC_COUNT, "DF_errors"))
+#endif
 {}
 
 
@@ -93,7 +102,7 @@ bool DataFlash_File::NeedErase(void)
 char *DataFlash_File::_log_file_name(uint16_t log_num)
 {
     char *buf = NULL;
-    asprintf(&buf, "%s/%u.bin", _log_directory, (unsigned)log_num);
+    asprintf(&buf, "%s/%u.BIN", _log_directory, (unsigned)log_num);
     return buf;
 }
 
@@ -104,7 +113,7 @@ char *DataFlash_File::_log_file_name(uint16_t log_num)
 char *DataFlash_File::_lastlog_file_name(void)
 {
     char *buf = NULL;
-    asprintf(&buf, "%s/lastlog.txt", _log_directory);
+    asprintf(&buf, "%s/LASTLOG.TXT", _log_directory);
     return buf;
 }
 
@@ -356,6 +365,7 @@ uint16_t DataFlash_File::start_new_log(void)
         _initialised = false;
         return 0xFFFF;
     }
+    _write_offset = 0;
 
     // now update lastlog.txt with the new log number
     fname = _lastlog_file_name();
@@ -508,35 +518,52 @@ void DataFlash_File::_io_timer(void)
         return;
     }
     uint32_t tnow = hal.scheduler->micros();
-    if (nbytes < 512 && 
+    if (nbytes < _writebuf_chunk && 
         tnow - _last_write_time < 2000000UL) {
         // write in 512 byte chunks, but always write at least once
         // per 2 seconds if data is available
         return;
     }
+
+    perf_begin(_perf_write);
+
     _last_write_time = tnow;
-    if (nbytes > 512) {
+    if (nbytes > _writebuf_chunk) {
         // be kind to the FAT PX4 filesystem
-        nbytes = 512;
+        nbytes = _writebuf_chunk;
     }
     if (_writebuf_head > _tail) {
         // only write to the end of the buffer
         nbytes = min(nbytes, _writebuf_size - _writebuf_head);
     }
+
+    // try to align writes on a 512 byte boundary to avoid filesystem
+    // reads
+    if ((nbytes + _write_offset) % 512 != 0) {
+        uint32_t ofs = (nbytes + _write_offset) % 512;
+        if (ofs < nbytes) {
+            nbytes -= ofs;
+        }
+    }
+
     assert(_writebuf_head+nbytes <= _writebuf_size);
     ssize_t nwritten = ::write(_write_fd, &_writebuf[_writebuf_head], nbytes);
     if (nwritten <= 0) {
-        //hal.console->printf("DataFlash write: %d %d\n", (int)nwritten, (int)errno);
+        perf_count(_perf_errors);
         close(_write_fd);
         _write_fd = -1;
         _initialised = false;
     } else {
+        _write_offset += nwritten;
         BUF_ADVANCEHEAD(_writebuf, nwritten);
         if (hal.scheduler->millis() - last_fsync_ms > 10000) {
             last_fsync_ms = hal.scheduler->millis();
+            perf_begin(_perf_fsync);
             ::fsync(_write_fd);            
+            perf_end(_perf_fsync);
         }
     }
+    perf_end(_perf_write);
 }
 
 #endif // HAL_OS_POSIX_IO
diff --git a/libraries/DataFlash/DataFlash_File.h b/libraries/DataFlash/DataFlash_File.h
index ea5a001da6..ee1ac1244d 100644
--- a/libraries/DataFlash/DataFlash_File.h
+++ b/libraries/DataFlash/DataFlash_File.h
@@ -10,6 +10,14 @@
 #ifndef DataFlash_File_h
 #define DataFlash_File_h
 
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+#include <systemlib/perf_counter.h>
+#else
+#define perf_begin(x)
+#define perf_end(x)
+#endif
+
+
 class DataFlash_File : public DataFlash_Class
 {
 public:
@@ -47,6 +55,7 @@ private:
     int _read_fd;
     uint16_t _read_fd_log_num;
     uint32_t _read_offset;
+    uint32_t _write_offset;
     volatile bool _initialised;
     const char *_log_directory;
     uint32_t last_fsync_ms;
@@ -59,6 +68,7 @@ private:
     // write buffer
     uint8_t *_writebuf;
     const uint16_t _writebuf_size;
+    const uint16_t _writebuf_chunk;
     volatile uint16_t _writebuf_head;
     volatile uint16_t _writebuf_tail;
     uint32_t _last_write_time;
@@ -72,6 +82,13 @@ private:
     void stop_logging(void);
 
     void _io_timer(void);
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+    // performance counters
+    perf_counter_t  _perf_write;
+    perf_counter_t  _perf_fsync;
+    perf_counter_t  _perf_errors;
+#endif
 };