From 6e1e89bb70764df5a2cd5a615e513d1cbb4a4ba3 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 28 Mar 2019 07:56:36 +1100
Subject: [PATCH] AP_FlashStorage: fixed a corruption bug

many thanks to @sh83 for tracking down this bug. When write_all() is
called with non-zero offset in init it ends up calling write with an
invalid offset, triggering a full reset

This fixes issue #10874
---
 libraries/AP_FlashStorage/AP_FlashStorage.cpp | 17 +++++++----------
 libraries/AP_FlashStorage/AP_FlashStorage.h   |  2 +-
 2 files changed, 8 insertions(+), 11 deletions(-)

diff --git a/libraries/AP_FlashStorage/AP_FlashStorage.cpp b/libraries/AP_FlashStorage/AP_FlashStorage.cpp
index 6f9259bd19..77c89ab145 100644
--- a/libraries/AP_FlashStorage/AP_FlashStorage.cpp
+++ b/libraries/AP_FlashStorage/AP_FlashStorage.cpp
@@ -17,6 +17,7 @@
 
 #include <AP_HAL/AP_HAL.h>
 #include <AP_FlashStorage/AP_FlashStorage.h>
+#include <AP_Math/AP_Math.h>
 #include <stdio.h>
 
 #define FLASHSTORAGE_DEBUG 0
@@ -111,12 +112,7 @@ bool AP_FlashStorage::init(void)
     // if the first sector is full then write out all data so we can erase it
     if (states[first_sector] == SECTOR_STATE_FULL) {
         current_sector = first_sector ^ 1;
-        // we start by writing all except the sector header
-        if (!write_all(sizeof(sector_header))) {
-            return erase_all();
-        }
-        // now write the header
-        if (!write(0, sizeof(sector_header))) {
+        if (!write_all()) {
             return erase_all();
         }
     }
@@ -314,13 +310,14 @@ bool AP_FlashStorage::erase_all(void)
 /*
   write all of mem_buffer to current sector
  */
-bool AP_FlashStorage::write_all(uint16_t start_ofs)
+bool AP_FlashStorage::write_all()
 {
     debug("write_all to sector %u at %u with reserved_space=%u\n",
            current_sector, write_offset, reserved_space);
-    for (uint16_t ofs=start_ofs; ofs<storage_size; ofs += max_write) {
-        if (!all_zero(ofs, max_write)) {
-            if (!write(ofs, max_write)) {
+    for (uint16_t ofs=0; ofs<storage_size; ofs += max_write) {
+        uint8_t n = MIN(max_write, storage_size-ofs);
+        if (!all_zero(ofs, n)) {
+            if (!write(ofs, n)) {
                 return false;
             }
         }
diff --git a/libraries/AP_FlashStorage/AP_FlashStorage.h b/libraries/AP_FlashStorage/AP_FlashStorage.h
index fb6be22aad..12c6c18de9 100644
--- a/libraries/AP_FlashStorage/AP_FlashStorage.h
+++ b/libraries/AP_FlashStorage/AP_FlashStorage.h
@@ -141,7 +141,7 @@ private:
     bool erase_all();
 
     // write all of mem_buffer to current sector
-    bool write_all(uint16_t start_ofs=0);
+    bool write_all();
 
     // return true if all bytes are zero
     bool all_zero(uint16_t ofs, uint16_t size);