HAL_PX4: implement optional flash backend for storage

this allows the last two flash sectors to be used as storage for
parameters, waypoints etc.

A change to the bootloader to not erase the last two sectors is needed

libraries/AP_HAL_PX4/Storage.cpp

@@ -9,6 +9,7 @@
 #include <errno.h>
 #include <stdio.h>
 #include <ctype.h>
+#include <nuttx/progmem.h>
 
 #include "Storage.h"
 using namespace PX4;
@@ -40,8 +41,12 @@ void PX4Storage::_storage_open(void)
 
     _dirty_mask.clearall();
 
-    // load from mtd
+    // load from storage backend
+#if USE_FLASH_STORAGE
+    _flash_load();
+#else
     _mtd_load();
+#endif
 
 #ifdef SAVE_STORAGE_FILE
     fd = open(SAVE_STORAGE_FILE, O_WRONLY|O_CREAT|O_TRUNC, 0644);
@@ -143,8 +148,12 @@ void PX4Storage::_timer_tick(void)
         return;
     }
 
-    // write the line
+    // save to storage backend
+#if USE_FLASH_STORAGE
+    _flash_write(i);
+#else
     _mtd_write(i);
+#endif
     
     perf_end(_perf_storage);
 }
@@ -198,4 +207,65 @@ void PX4Storage::_mtd_load(void)
     close(fd);
 }
 
+/*
+  load all data from flash
+ */
+void PX4Storage::_flash_load(void)
+{
+    _flash_page = up_progmem_npages() - 2;
+    if (up_progmem_pagesize(_flash_page) != 128*1024U ||
+        up_progmem_pagesize(_flash_page+1) != 128*1024U) {
+        AP_HAL::panic("Bad flash page sizes %u %u",
+                      up_progmem_pagesize(_flash_page),
+                      up_progmem_pagesize(_flash_page+1));
+    }
+        
+    printf("Storage: Using flash pages %u and %u\n", _flash_page, _flash_page+1);
+    
+    if (!_flash.init()) {
+        AP_HAL::panic("unable to init flash storage");
+    }
+}
+
+/*
+  write one storage line. This also updates _dirty_mask. 
+*/
+void PX4Storage::_flash_write(uint16_t line)
+{
+    if (_flash.write(line*PX4_STORAGE_LINE_SIZE, PX4_STORAGE_LINE_SIZE)) {
+        // mark the line clean
+        _dirty_mask.clear(line);        
+    } else {
+        perf_count(_perf_errors);
+    }
+}
+
+/*
+  callback to write data to flash
+ */
+bool PX4Storage::_flash_write_data(uint8_t sector, uint32_t offset, const uint8_t *data, uint16_t length)
+{
+    size_t base_address = up_progmem_getaddress(_flash_page+sector);
+    return up_progmem_write(base_address+offset, data, length) == length;
+}
+
+/*
+  callback to read data from flash
+ */
+bool PX4Storage::_flash_read_data(uint8_t sector, uint32_t offset, uint8_t *data, uint16_t length)
+{
+    size_t base_address = up_progmem_getaddress(_flash_page+sector);
+    const uint8_t *b = ((const uint8_t *)base_address)+offset;
+    memcpy(data, b, length);
+    return true;
+}
+
+/*
+  callback to erase flash sector
+ */
+bool PX4Storage::_flash_erase_sector(uint8_t sector)
+{
+    return up_progmem_erasepage(_flash_page+sector) > 0;
+}
+
 #endif // CONFIG_HAL_BOARD

libraries/AP_HAL_PX4/Storage.h

@@ -1,12 +1,23 @@
 #pragma once
 
+#define USE_FLASH_STORAGE 1
+
 #include <AP_HAL/AP_HAL.h>
 #include "AP_HAL_PX4_Namespace.h"
 #include <systemlib/perf_counter.h>
 #include <AP_Common/Bitmask.h>
+#include <AP_FlashStorage/AP_FlashStorage.h>
 
 #define PX4_STORAGE_SIZE HAL_STORAGE_SIZE
+
+#if USE_FLASH_STORAGE
+// when using flash storage we use a small line size to make storage
+// compact and minimise the number of erase cycles needed
+#define PX4_STORAGE_LINE_SHIFT 3
+#else
 #define PX4_STORAGE_LINE_SHIFT 9
+#endif
+
 #define PX4_STORAGE_LINE_SIZE (1<<PX4_STORAGE_LINE_SHIFT)
 #define PX4_STORAGE_NUM_LINES (PX4_STORAGE_SIZE/PX4_STORAGE_LINE_SIZE)
 
@@ -33,9 +44,22 @@ private:
     int _fd = -1;
     void _mtd_load(void);
     void _mtd_write(uint16_t line);
-    
 #if defined (CONFIG_ARCH_BOARD_PX4FMU_V4)
     irqstate_t irq_state;
 #endif
     void bus_lock(bool lock);
+
+    bool _flash_write_data(uint8_t sector, uint32_t offset, const uint8_t *data, uint16_t length);
+    bool _flash_read_data(uint8_t sector, uint32_t offset, uint8_t *data, uint16_t length);
+    bool _flash_erase_sector(uint8_t sector);
+    uint8_t _flash_page;
+    
+    AP_FlashStorage _flash{_buffer,
+            128*1024U, 
+            FUNCTOR_BIND_MEMBER(&PX4Storage::_flash_write_data, bool, uint8_t, uint32_t, const uint8_t *, uint16_t),
+            FUNCTOR_BIND_MEMBER(&PX4Storage::_flash_read_data, bool, uint8_t, uint32_t, uint8_t *, uint16_t),
+            FUNCTOR_BIND_MEMBER(&PX4Storage::_flash_erase_sector, bool, uint8_t)};
+    
+    void _flash_load(void);
+    void _flash_write(uint16_t line);
 };