From 03a033c5c459102b5c1569b076ffafa16e0fd6f5 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 13 Jan 2021 10:09:53 +1100
Subject: [PATCH] StorageManager: added support for 32k storage with param
 backup

when HAL_STORAGE_SIZE == 32768 then add:

 - 1280 more bytes for params
 - double waypoint space
 - add a parameter backup area
---
 libraries/StorageManager/StorageManager.cpp | 35 +++++++++++++++++++++
 libraries/StorageManager/StorageManager.h   | 10 ++++--
 2 files changed, 43 insertions(+), 2 deletions(-)

diff --git a/libraries/StorageManager/StorageManager.cpp b/libraries/StorageManager/StorageManager.cpp
index 788a34058b..79065cb99c 100644
--- a/libraries/StorageManager/StorageManager.cpp
+++ b/libraries/StorageManager/StorageManager.cpp
@@ -21,6 +21,7 @@
 
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Vehicle/AP_Vehicle_Type.h>
+#include <AP_Math/AP_Math.h>
 
 #include "StorageManager.h"
 
@@ -76,6 +77,12 @@ const StorageManager::StorageArea StorageManager::layout[STORAGE_NUM_AREAS] = {
     { StorageFence,    9772,   256},
     { StorageMission,  10028,  5204}, // leave 128 byte gap for expansion
     { StorageCANDNA,   15232,  1024},
+    // 128 byte gap at end of first 16k
+#endif
+#if STORAGE_NUM_AREAS >= 19
+    { StorageParam,    16384, 1280},
+    { StorageMission,  17664, 9842},
+    { StorageParamBak, 27506, 5376},
 #endif
 };
 
@@ -113,6 +120,12 @@ const StorageManager::StorageArea StorageManager::layout[STORAGE_NUM_AREAS] = {
     { StorageFence,    9772,   256},
     { StorageMission,  10028,  5204}, // leave 128 byte gap for expansion
     { StorageCANDNA,   15232,  1024},
+    // 128 byte gap at end of first 16k
+#endif
+#if STORAGE_NUM_AREAS >= 19
+    { StorageParam,    16384, 1280},
+    { StorageMission,  17664, 9842},
+    { StorageParamBak, 27506, 5376},
 #endif
 };
 #endif // STORAGE_NUM_AREAS == 1
@@ -275,3 +288,25 @@ void StorageAccess::write_uint32(uint16_t loc, uint32_t value) const
 {
     write_block(loc, &value, sizeof(value));
 }
+
+/*
+  copy one area to another
+ */
+bool StorageAccess::copy_area(const StorageAccess &source)
+{
+    // we deliberately allow for copies from smaller areas. This
+    // allows for a partial backup region for parameters
+    uint16_t total = MIN(source.size(), size());
+    uint16_t ofs = 0;
+    while (total > 0) {
+        uint8_t block[32];
+        uint16_t n = MIN(sizeof(block), total);
+        if (!source.read_block(block, ofs, n) ||
+            !write_block(ofs, block, n)) {
+            return false;
+        }
+        total -= n;
+        ofs += n;
+    }
+    return true;
+}
diff --git a/libraries/StorageManager/StorageManager.h b/libraries/StorageManager/StorageManager.h
index 2350a65fa3..f427f324b7 100644
--- a/libraries/StorageManager/StorageManager.h
+++ b/libraries/StorageManager/StorageManager.h
@@ -26,7 +26,9 @@
   use just one area per storage type for boards with 4k of
   storage. Use larger areas for other boards
  */
-#if HAL_STORAGE_SIZE >= 16384
+#if HAL_STORAGE_SIZE >= 32768
+#define STORAGE_NUM_AREAS 19
+#elif HAL_STORAGE_SIZE >= 16384
 #define STORAGE_NUM_AREAS 15
 #elif HAL_STORAGE_SIZE >= 15360 && defined(HAL_NUM_CAN_IFACES)
 #define STORAGE_NUM_AREAS 12
@@ -55,7 +57,8 @@ public:
         StorageMission = 3,
         StorageKeys    = 4,
         StorageBindInfo= 5,
-        StorageCANDNA  = 6
+        StorageCANDNA  = 6,
+        StorageParamBak = 7
     };
 
     // erase whole of storage
@@ -98,6 +101,9 @@ public:
     void write_uint16(uint16_t loc, uint16_t value) const;
     void write_uint32(uint16_t loc, uint32_t value) const;
 
+    // copy from one storage area to another
+    bool copy_area(const StorageAccess &source);
+
 private:
     const StorageManager::StorageType type;
     uint16_t total_size;