From eb6848b8c2bb6c0f11cc1115f05d5b880efece13 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 23 Feb 2016 16:32:19 +1100
Subject: [PATCH] HAL_PX4: work around a bus locking issue on Pixracer

Pixracer has FRAM on the same bus as the ms5611 and the FRAM ramtron
driver does not use the same locking mechanism as other px4 SPI
drivers. We need to disable interrupts during FRAM transfers to ensure
we don't get FRAM corruption
---
 libraries/AP_HAL_PX4/Storage.cpp | 43 +++++++++++++++++++++++++++-----
 libraries/AP_HAL_PX4/Storage.h   |  5 ++++
 2 files changed, 42 insertions(+), 6 deletions(-)

diff --git a/libraries/AP_HAL_PX4/Storage.cpp b/libraries/AP_HAL_PX4/Storage.cpp
index 3024b8b675..f7e07aac73 100644
--- a/libraries/AP_HAL_PX4/Storage.cpp
+++ b/libraries/AP_HAL_PX4/Storage.cpp
@@ -51,9 +51,11 @@ uint32_t PX4Storage::_mtd_signature(void)
     if (lseek(mtd_fd, MTD_SIGNATURE_OFFSET, SEEK_SET) != MTD_SIGNATURE_OFFSET) {
         hal.scheduler->panic("Failed to seek in " MTD_PARAMS_FILE);
     }
+    bus_lock(true);
     if (read(mtd_fd, &v, sizeof(v)) != sizeof(v)) {
         hal.scheduler->panic("Failed to read signature from " MTD_PARAMS_FILE);
     }
+    bus_lock(false);
     close(mtd_fd);
     return v;
 }
@@ -71,9 +73,11 @@ void PX4Storage::_mtd_write_signature(void)
     if (lseek(mtd_fd, MTD_SIGNATURE_OFFSET, SEEK_SET) != MTD_SIGNATURE_OFFSET) {
         hal.scheduler->panic("Failed to seek in " MTD_PARAMS_FILE);
     }
+    bus_lock(true);
     if (write(mtd_fd, &v, sizeof(v)) != sizeof(v)) {
         hal.scheduler->panic("Failed to write signature in " MTD_PARAMS_FILE);
     }
+    bus_lock(false);
     close(mtd_fd);
 }
 
@@ -103,10 +107,12 @@ void PX4Storage::_upgrade_to_mtd(void)
     }
     close(old_fd);
     ssize_t ret;
+    bus_lock(true);
     if ((ret=::write(mtd_fd, _buffer, sizeof(_buffer))) != sizeof(_buffer)) {
         ::printf("mtd write of %u bytes returned %d errno=%d\n", sizeof(_buffer), ret, errno);
         hal.scheduler->panic("Unable to write MTD for upgrade");        
     }
+    bus_lock(false);
     close(mtd_fd);
 #if STORAGE_RENAME_OLD_FILE
     rename(OLD_STORAGE_FILE, OLD_STORAGE_FILE_BAK);
@@ -156,7 +162,9 @@ void PX4Storage::_storage_open(void)
 	}
         const uint16_t chunk_size = 128;
         for (uint16_t ofs=0; ofs<sizeof(_buffer); ofs += chunk_size) {
+            bus_lock(true);
             ssize_t ret = read(fd, &_buffer[ofs], chunk_size);
+            bus_lock(false);
             if (ret != chunk_size) {
                 ::printf("storage read of %u bytes at %u to %p failed - got %d errno=%d\n",
                          (unsigned)sizeof(_buffer), (unsigned)ofs, &_buffer[ofs], (int)ret, (int)errno);
@@ -205,6 +213,26 @@ void PX4Storage::write_block(uint16_t loc, const void *src, size_t n)
 	}
 }
 
+void PX4Storage::bus_lock(bool lock)
+{
+#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4)
+    /*
+      this is needed on Pixracer where the ms5611 may be on the same
+      bus as FRAM, and the NuttX ramtron driver does not go via the
+      PX4 spi bus abstraction. The ramtron driver relies on
+      SPI_LOCK(). We need to prevent the ms5611 reads which happen in
+      interrupt context from interfering with the FRAM operations. As
+      the px4 spi bus abstraction just uses interrupt blocking as the
+      locking mechanism we need to block interrupts here as well.
+     */
+    if (lock) {
+        irq_state = irqsave();
+    } else {
+        irqrestore(irq_state);
+    }
+#endif
+}
+
 void PX4Storage::_timer_tick(void)
 {
 	if (!_initialised || _dirty_mask == 0) {
@@ -254,12 +282,15 @@ void PX4Storage::_timer_tick(void)
 	 */
 	if (lseek(_fd, i<<PX4_STORAGE_LINE_SHIFT, SEEK_SET) == (i<<PX4_STORAGE_LINE_SHIFT)) {
 		_dirty_mask &= ~write_mask;
-		if (write(_fd, &_buffer[i<<PX4_STORAGE_LINE_SHIFT], n<<PX4_STORAGE_LINE_SHIFT) != n<<PX4_STORAGE_LINE_SHIFT) {
-			// write error - likely EINTR
-			_dirty_mask |= write_mask;
-			close(_fd);
-			_fd = -1;
-			perf_count(_perf_errors);
+                bus_lock(true);
+		ssize_t ret = write(_fd, &_buffer[i<<PX4_STORAGE_LINE_SHIFT], n<<PX4_STORAGE_LINE_SHIFT);
+                bus_lock(false);
+                if (ret != n<<PX4_STORAGE_LINE_SHIFT) {
+                    // write error - likely EINTR
+                    _dirty_mask |= write_mask;
+                    close(_fd);
+                    _fd = -1;
+                    perf_count(_perf_errors);
 		}
 	}
 	perf_end(_perf_storage);
diff --git a/libraries/AP_HAL_PX4/Storage.h b/libraries/AP_HAL_PX4/Storage.h
index 35051e8d49..13c83c8b15 100644
--- a/libraries/AP_HAL_PX4/Storage.h
+++ b/libraries/AP_HAL_PX4/Storage.h
@@ -37,6 +37,11 @@ private:
     void _upgrade_to_mtd(void);
     uint32_t _mtd_signature(void);
     void _mtd_write_signature(void);
+
+#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4)
+    irqstate_t irq_state;
+#endif
+    void bus_lock(bool lock);
 };
 
 #endif // __AP_HAL_PX4_STORAGE_H__