From f90962aa0c02fc2ad30fb9962a5236c044db29e8 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 13 Jan 2021 10:10:27 +1100
Subject: [PATCH] AP_Param: support restoring from parameter backup region

if header on primary parameter storage is corrupt then restore from
backup
---
 libraries/AP_Param/AP_Param.cpp | 20 +++++++++++++++++++-
 libraries/AP_Param/AP_Param.h   |  1 +
 2 files changed, 20 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_Param/AP_Param.cpp b/libraries/AP_Param/AP_Param.cpp
index 0e3233ba23..f23786d59d 100644
--- a/libraries/AP_Param/AP_Param.cpp
+++ b/libraries/AP_Param/AP_Param.cpp
@@ -32,6 +32,7 @@
 #include <GCS_MAVLink/GCS.h>
 #include <StorageManager/StorageManager.h>
 #include <AP_BoardConfig/AP_BoardConfig.h>
+#include <AP_InternalError/AP_InternalError.h>
 #include <stdio.h>
 
 extern const AP_HAL::HAL &hal;
@@ -101,6 +102,9 @@ const AP_Param::param_defaults_struct AP_Param::param_defaults_data = {
 // storage object
 StorageAccess AP_Param::_storage(StorageManager::StorageParam);
 
+// backup storage object
+StorageAccess AP_Param::_storage_bak(StorageManager::StorageParamBak);
+
 // flags indicating frame type
 uint16_t AP_Param::_frame_type_flags;
 
@@ -108,6 +112,7 @@ uint16_t AP_Param::_frame_type_flags;
 void AP_Param::eeprom_write_check(const void *ptr, uint16_t ofs, uint8_t size)
 {
     _storage.write_block(ofs, ptr, size);
+    _storage_bak.write_block(ofs, ptr, size);
 }
 
 bool AP_Param::_hide_disabled_groups = true;
@@ -314,19 +319,32 @@ bool AP_Param::check_var_info(void)
 // setup the _var_info[] table
 bool AP_Param::setup(void)
 {
-    struct EEPROM_header hdr;
+    struct EEPROM_header hdr {};
+    struct EEPROM_header hdr2 {};
 
     // check the header
     _storage.read_block(&hdr, 0, sizeof(hdr));
+    _storage_bak.read_block(&hdr2, 0, sizeof(hdr2));
     if (hdr.magic[0] != k_EEPROM_magic0 ||
         hdr.magic[1] != k_EEPROM_magic1 ||
         hdr.revision != k_EEPROM_revision) {
+        if (hdr2.magic[0] == k_EEPROM_magic0 &&
+            hdr2.magic[1] == k_EEPROM_magic1 &&
+            hdr2.revision == k_EEPROM_revision &&
+            _storage.copy_area(_storage_bak)) {
+            // restored from backup
+            INTERNAL_ERROR(AP_InternalError::error_t::params_restored);
+            return true;
+        }
         // header doesn't match. We can't recover any variables. Wipe
         // the header and setup the sentinal directly after the header
         Debug("bad header in setup - erasing");
         erase_all();
     }
 
+    // ensure that backup is in sync with primary
+    _storage_bak.copy_area(_storage);
+
     return true;
 }
 
diff --git a/libraries/AP_Param/AP_Param.h b/libraries/AP_Param/AP_Param.h
index e795da62ea..ff7d0a5cf8 100644
--- a/libraries/AP_Param/AP_Param.h
+++ b/libraries/AP_Param/AP_Param.h
@@ -641,6 +641,7 @@ private:
     void send_parameter(const char *name, enum ap_var_type param_header_type, uint8_t idx) const;
 
     static StorageAccess        _storage;
+    static StorageAccess        _storage_bak;
     static uint16_t             _num_vars;
     static uint16_t             _parameter_count;
     static const struct Info *  _var_info;