From 01472aa795d03a8c0d71a0aea3ad414e5d9a4bab Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 27 May 2019 11:39:41 +1000
Subject: [PATCH] AP_FlashStorage: added support for STM32F1xx

---
 libraries/AP_FlashStorage/AP_FlashStorage.cpp |  2 ++
 libraries/AP_FlashStorage/AP_FlashStorage.h   | 25 +++++++++++++++++++
 .../examples/FlashTest/FlashTest.cpp          | 21 ++++++++++++++--
 3 files changed, 46 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_FlashStorage/AP_FlashStorage.cpp b/libraries/AP_FlashStorage/AP_FlashStorage.cpp
index 1754a349bf..b8e2e99324 100644
--- a/libraries/AP_FlashStorage/AP_FlashStorage.cpp
+++ b/libraries/AP_FlashStorage/AP_FlashStorage.cpp
@@ -187,9 +187,11 @@ bool AP_FlashStorage::write(uint16_t offset, uint16_t length)
         uint16_t block_ofs = header.block_num*block_size;
         uint16_t block_nbytes = (header.num_blocks_minus_one+1)*block_size;
         
+#if AP_FLASHSTORAGE_MULTI_WRITE
         if (!flash_write(current_sector, write_offset, (uint8_t*)&header, sizeof(header))) {
             return false;
         }
+#endif
         if (!flash_write(current_sector, write_offset+sizeof(header), &mem_buffer[block_ofs], block_nbytes)) {
             return false;
         }
diff --git a/libraries/AP_FlashStorage/AP_FlashStorage.h b/libraries/AP_FlashStorage/AP_FlashStorage.h
index 12c6c18de9..1ee8897f15 100644
--- a/libraries/AP_FlashStorage/AP_FlashStorage.h
+++ b/libraries/AP_FlashStorage/AP_FlashStorage.h
@@ -39,6 +39,16 @@
 
 #include <AP_HAL/AP_HAL.h>
 
+#if defined(STM32F1)
+/*
+  the STM32F1 can't change individual bits from 1 to 0 unless all bits in
+  the 16 bit word are 1
+ */
+#define AP_FLASHSTORAGE_MULTI_WRITE 0
+#else
+#define AP_FLASHSTORAGE_MULTI_WRITE 1
+#endif
+
 /*
   The StorageManager holds the layout of non-volatile storeage
  */
@@ -99,19 +109,34 @@ private:
     bool write_error;
 
     // 24 bit signature
+#if AP_FLASHSTORAGE_MULTI_WRITE
     static const uint32_t signature = 0x51685B;
+#else
+    static const uint32_t signature = 0x51;
+#endif
 
     // 8 bit sector states
     enum SectorState {
+#if AP_FLASHSTORAGE_MULTI_WRITE
         SECTOR_STATE_AVAILABLE = 0xFF,
         SECTOR_STATE_IN_USE    = 0xFE,
         SECTOR_STATE_FULL      = 0xFC
+#else
+        SECTOR_STATE_AVAILABLE = 0xFFFFFFFF,
+        SECTOR_STATE_IN_USE    = 0xFFFFFFF1,
+        SECTOR_STATE_FULL      = 0xFFF2FFF1,
+#endif
     };
 
     // header in first word of each sector
     struct sector_header {
+#if AP_FLASHSTORAGE_MULTI_WRITE
         uint32_t state:8;
         uint32_t signature:24;
+#else
+        uint32_t state:32;
+        uint32_t signature:16;
+#endif
     };
 
 
diff --git a/libraries/AP_FlashStorage/examples/FlashTest/FlashTest.cpp b/libraries/AP_FlashStorage/examples/FlashTest/FlashTest.cpp
index fd52c8aa4c..8dbf003311 100644
--- a/libraries/AP_FlashStorage/examples/FlashTest/FlashTest.cpp
+++ b/libraries/AP_FlashStorage/examples/FlashTest/FlashTest.cpp
@@ -54,8 +54,25 @@ bool FlashTest::flash_write(uint8_t sector, uint32_t offset, const uint8_t *data
                       (unsigned)length);
     }
     uint8_t *b = &flash[sector][offset];
-    for (uint16_t i=0; i<length; i++) {
-        b[i] &= data[i];
+    if ((offset & 1) || (length & 1)) {
+        AP_HAL::panic("FATAL: invalid write at %u:%u len=%u\n",
+                      sector, offset, length);
+    }
+    const uint16_t *data16 = (const uint16_t *)data;
+    uint16_t *b16 = (uint16_t *)&b[0];
+    uint16_t len16 = length/2;
+    for (uint16_t i=0; i<len16; i++) {
+        if (data16[i] & !b16[i]) {
+            AP_HAL::panic("FATAL: invalid write16 at %u:%u 0x%04x 0x%04x\n",
+                          sector, offset+i, b[i], data[i]);
+        }
+#if !AP_FLASHSTORAGE_MULTI_WRITE
+        if (data16[i] != b16[i] && data16[i] != 0xFFFF && b16[i] != 0xFFFF) {
+            AP_HAL::panic("FATAL: invalid write16 at %u:%u 0x%04x 0x%04x\n",
+                          sector, offset+i, b[i], data[i]);
+        }
+#endif
+        b16[i] &= data16[i];
     }
     return true;
 }