From 02d7867d7950e8d9365e3363f6d511b1076c4207 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 17 Dec 2015 09:23:14 +1100
Subject: [PATCH] DataFlash: added locking for multi-thread logging support

---
 libraries/DataFlash/DataFlash_File.cpp | 15 +++++++++++++++
 libraries/DataFlash/DataFlash_File.h   |  2 ++
 2 files changed, 17 insertions(+)

diff --git a/libraries/DataFlash/DataFlash_File.cpp b/libraries/DataFlash/DataFlash_File.cpp
index ae906693ea..ebfd712f93 100644
--- a/libraries/DataFlash/DataFlash_File.cpp
+++ b/libraries/DataFlash/DataFlash_File.cpp
@@ -96,6 +96,12 @@ void DataFlash_File::Init()
     int ret;
     struct stat st;
 
+    semaphore = hal.util->new_semaphore();
+    if (semaphore == nullptr) {
+        hal.console->printf("Failed to create DataFlash_File semaphore\n");
+        return;
+    }
+    
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
     // try to cope with an existing lowercase log directory
     // name. NuttX does not handle case insensitive VFAT well
@@ -437,8 +443,13 @@ bool DataFlash_File::WritePrioritisedBlock(const void *pBuffer, uint16_t size, b
         return false;
     }
 
+    if (!semaphore->take(1)) {
+        return false;
+    }
+    
     if (! WriteBlockCheckStartupMessages()) {
         _dropped++;
+        semaphore->give();
         return false;
     }
 
@@ -453,12 +464,14 @@ bool DataFlash_File::WritePrioritisedBlock(const void *pBuffer, uint16_t size, b
         // things:
         if (space < non_messagewriter_message_reserved_space()) {
             // this message isn't dropped, it will be sent again...
+            semaphore->give();
             return false;
         }
     } else {
         // we reserve some amount of space for critical messages:
         if (!is_critical && space < critical_message_reserved_space()) {
             _dropped++;
+            semaphore->give();
             return false;
         }
     }
@@ -467,6 +480,7 @@ bool DataFlash_File::WritePrioritisedBlock(const void *pBuffer, uint16_t size, b
     if (space < size) {
         hal.util->perf_count(_perf_overruns);
         _dropped++;
+        semaphore->give();
         return false;
     }
 
@@ -490,6 +504,7 @@ bool DataFlash_File::WritePrioritisedBlock(const void *pBuffer, uint16_t size, b
             BUF_ADVANCETAIL(_writebuf, n);
         }
     }
+    semaphore->give();
     return true;
 }
 
diff --git a/libraries/DataFlash/DataFlash_File.h b/libraries/DataFlash/DataFlash_File.h
index e8818782fc..f1526cbbb4 100644
--- a/libraries/DataFlash/DataFlash_File.h
+++ b/libraries/DataFlash/DataFlash_File.h
@@ -131,6 +131,8 @@ private:
         return ret;
     };
 
+    AP_HAL::Semaphore *semaphore;
+    
     // performance counters
     AP_HAL::Util::perf_counter_t  _perf_write;
     AP_HAL::Util::perf_counter_t  _perf_fsync;