AP_Logger: mavlink backend needs to be the last backend

be really careful to catch aborted erases
take care to protect shared structures in io thread
if flash corruption is detected try and recover whole files
overwrite format in erase to make sure erase happens
output useful messages at critical times
a block is 64k a sector is 4k, rename internal variables appropriately
cope with log wrapping when sending log listings over mavlink

libraries/AP_Logger/AP_Logger.cpp

@@ -144,8 +144,8 @@ void AP_Logger::Init(const struct LogStructure *structures, uint8_t num_types)
     }
 #endif // HAVE_FILESYSTEM_SUPPORT
 
-#if LOGGER_MAVLINK_SUPPORT
-    if (_params.backend_types & uint8_t(Backend_Type::MAVLINK)) {
+#ifdef HAL_LOGGING_DATAFLASH
+    if (_params.backend_types & uint8_t(Backend_Type::BLOCK)) {
         if (_next_backend == LOGGER_MAX_BACKENDS) {
             AP_HAL::panic("Too many backends");
             return;
@@ -153,11 +153,10 @@ void AP_Logger::Init(const struct LogStructure *structures, uint8_t num_types)
         LoggerMessageWriter_DFLogStart *message_writer =
             new LoggerMessageWriter_DFLogStart();
         if (message_writer != nullptr)  {
-            backends[_next_backend] = new AP_Logger_MAVLink(*this,
-                                                            message_writer);
+            backends[_next_backend] = new AP_Logger_DataFlash(*this, message_writer);
         }
         if (backends[_next_backend] == nullptr) {
-            hal.console->printf("Unable to open AP_Logger_MAVLink");
+            hal.console->printf("Unable to open AP_Logger_DataFlash");
         } else {
             _next_backend++;
         }
@@ -182,9 +181,9 @@ void AP_Logger::Init(const struct LogStructure *structures, uint8_t num_types)
         }
     }
 #endif
-
-#ifdef HAL_LOGGING_DATAFLASH
-    if (_params.backend_types & uint8_t(Backend_Type::BLOCK)) {
+    // the "main" logging type needs to come before mavlink so that index 0 is correct
+#if LOGGER_MAVLINK_SUPPORT
+    if (_params.backend_types & uint8_t(Backend_Type::MAVLINK)) {
         if (_next_backend == LOGGER_MAX_BACKENDS) {
             AP_HAL::panic("Too many backends");
             return;
@@ -192,16 +191,17 @@ void AP_Logger::Init(const struct LogStructure *structures, uint8_t num_types)
         LoggerMessageWriter_DFLogStart *message_writer =
             new LoggerMessageWriter_DFLogStart();
         if (message_writer != nullptr)  {
-            backends[_next_backend] = new AP_Logger_DataFlash(*this, message_writer);
+            backends[_next_backend] = new AP_Logger_MAVLink(*this,
+                                                            message_writer);
         }
         if (backends[_next_backend] == nullptr) {
-            hal.console->printf("Unable to open AP_Logger_DataFlash");
+            hal.console->printf("Unable to open AP_Logger_MAVLink");
         } else {
             _next_backend++;
         }
     }
 #endif
-    
+
     for (uint8_t i=0; i<_next_backend; i++) {
         backends[i]->Init();
     }

libraries/AP_Logger/AP_Logger_Block.cpp

@@ -6,6 +6,7 @@
 
 #include <AP_HAL/AP_HAL.h>
 #include <stdio.h>
+#include <GCS_MAVLink/GCS.h>
 
 extern AP_HAL::HAL& hal;
 
@@ -28,7 +29,7 @@ void AP_Logger_Block::Init(void)
 {
     if (CardInserted()) {
         // reserve space for version in last sector
-        df_NumPages -= df_PagePerSector;
+        df_NumPages -= df_PagePerBlock;
 
         // determine and limit file backend buffersize
         uint32_t bufsize = _front._params.file_bufsize;
@@ -38,7 +39,7 @@ void AP_Logger_Block::Init(void)
         bufsize *= 1024;
 
         // If we can't allocate the full size, try to reduce it until we can allocate it
-        while (!writebuf.set_size(bufsize) && bufsize >= df_PageSize * df_PagePerSector) {
+        while (!writebuf.set_size(bufsize) && bufsize >= df_PageSize * df_PagePerBlock) {
             hal.console->printf("AP_Logger_Block: Couldn't set buffer size to=%u\n", (unsigned)bufsize);
             bufsize >>= 1;
         }
@@ -85,8 +86,8 @@ void AP_Logger_Block::FinishWrite(void)
     }
 
     // when starting a new sector, erase it
-    if ((df_PageAdr-1) % df_PagePerSector == 0) {
-        SectorErase(df_PageAdr / df_PagePerSector);
+    if ((df_PageAdr-1) % df_PagePerBlock == 0) {
+        SectorErase(df_PageAdr / df_PagePerBlock);
     }
 }
 
@@ -198,6 +199,11 @@ uint16_t AP_Logger_Block::GetFileNumber()
 void AP_Logger_Block::EraseAll()
 {
     WITH_SEMAPHORE(sem);
+    gcs().send_text(MAV_SEVERITY_INFO, "Chip erase started");
+    // reset the format version so that any incomplete erase will be caught
+    Sector4kErase(get_sector(df_NumPages+1));
+    // reset the wrapped status
+    Sector4kErase(get_sector(df_NumPages));
 
     if (erase_started) {
         // already erasing
@@ -225,6 +231,7 @@ void AP_Logger_Block::Prep()
     if (NeedErase()) {
         EraseAll();
     }
+    validate_log_structure();
 }
 
 /*
@@ -243,6 +250,51 @@ bool AP_Logger_Block::NeedErase(void)
     return true;
 }
 
+/*
+ * iterate through all of the logs files looking for ones that are corrupted and correct.
+ */
+void AP_Logger_Block::validate_log_structure()
+{
+    WITH_SEMAPHORE(sem);
+    bool wrapped = check_wrapped();
+    uint32_t page = 1;
+    uint32_t page_start = 1;
+
+    StartRead(page);
+    uint16_t file = GetFileNumber();
+    uint16_t first_file = file;
+    uint16_t next_file = file;
+    uint16_t last_file;
+
+    while (file != 0xFFFF && page <= df_NumPages && (file == next_file || (wrapped && file < next_file))) {
+        uint32_t end_page = find_last_page_of_log(file);
+        if (end_page == 0 || end_page < page) { // this can happen and may be responsible for corruption that we have seen
+            break;
+        }
+        page = end_page + 1;
+        StartRead(page);
+        file = GetFileNumber();
+        next_file++;
+        if (wrapped && file < next_file) {
+            page_start = page;
+            next_file = file;
+            first_file = file;
+        } else if (last_file < next_file) {
+            last_file = file;
+        }
+        if (file == next_file) {
+            hal.console->printf("Found complete log %d at %X-%X\n", int(file), unsigned(page), unsigned(find_last_page_of_log(file)));
+        }
+    }
+
+    if (file != 0xFFFF && file != next_file && page <= df_NumPages && page > 0) {
+        gcs().send_text(MAV_SEVERITY_WARNING, "Found corrupt log %d at 0x%04X, erasing", int(file), unsigned(page));
+        df_EraseFrom = page;
+    } else if (next_file != 0xFFFF && page > 0 && next_file > 1) { // chip is empty
+        gcs().send_text(MAV_SEVERITY_INFO, "Found %d complete logs at 0x%04X-0x%04X", int(next_file - first_file), unsigned(page_start), unsigned(page - 1));
+    }
+}
+
 /**
   get raw data from a log
  */
@@ -327,28 +379,23 @@ uint16_t AP_Logger_Block::get_num_logs(void)
     WITH_SEMAPHORE(sem);
     uint32_t lastpage;
     uint32_t last;
-    uint32_t first;
 
     if (!CardInserted() || find_last_page() == 1) {
         return 0;
     }
 
     StartRead(1);
-
-    if (GetFileNumber() == 0xFFFF) {
+    uint32_t first = GetFileNumber();
+    
+    if (first == 0xFFFF) {
         return 0;
     }
 
     lastpage = find_last_page();
     StartRead(lastpage);
     last = GetFileNumber();
-    StartRead(lastpage + 2);
-    if (GetFileNumber() == 0xFFFF) {
-        StartRead(((((lastpage-1)>>8)+1)<<8)+1);    // next sector
-    }
-    first = GetFileNumber();
-    if (first > last) {
-        StartRead(1);
+    if (check_wrapped()) {
+        StartRead(lastpage + 1);
         first = GetFileNumber();
     }
 
@@ -411,7 +458,6 @@ void AP_Logger_Block::get_log_boundaries(uint16_t log_num, uint32_t & start_page
             end_page = find_last_page_of_log((uint16_t)log_num);
             start_page = end_page + 1;
         }
-
     } else {
         if (log_num==1) {
             StartRead(df_NumPages);
@@ -449,7 +495,7 @@ bool AP_Logger_Block::check_wrapped(void)
 }
 
 
-// This funciton finds the last log number
+// This function finds the last log number
 uint16_t AP_Logger_Block::find_last_log(void)
 {
     WITH_SEMAPHORE(sem);
@@ -558,6 +604,7 @@ uint32_t AP_Logger_Block::find_last_page_of_log(uint16_t log_number)
         return bottom;
     }
 
+    gcs().send_text(MAV_SEVERITY_ERROR, "Missing last page of log %d at top=%X or bottom=%X", int(log_number), unsigned(top), unsigned(bottom));
     return 0;
 }
 
@@ -606,6 +653,7 @@ void AP_Logger_Block::io_timer(void)
         if (InErase()) {
             return;
         }
+        WITH_SEMAPHORE(sem);
         // write the logging format in the last page
         StartWrite(df_NumPages+1);
         uint32_t version = DF_LOGGING_FORMAT;
@@ -613,6 +661,28 @@ void AP_Logger_Block::io_timer(void)
         memcpy(buffer, &version, sizeof(version));
         FinishWrite();
         erase_started = false;
+        gcs().send_text(MAV_SEVERITY_INFO, "Chip erase complete");
+    }
+
+    if (df_EraseFrom > 0) {
+        WITH_SEMAPHORE(sem);
+
+        const uint32_t sectors = df_NumPages / df_PagePerSector;
+        const uint32_t sectors_in_64k = 0x10000 / (df_PagePerSector * df_PageSize);
+        uint32_t next_sector = get_sector(df_EraseFrom);
+        const uint32_t aligned_sector = sectors - (((df_NumPages - df_EraseFrom + 1) / df_PagePerSector) / sectors_in_64k) * sectors_in_64k;
+        while (next_sector < aligned_sector) {
+            Sector4kErase(next_sector);
+            next_sector++;
+        }
+        uint16_t blocks_erased = 0;
+        while (next_sector < sectors) {
+            blocks_erased++;
+            SectorErase(next_sector / sectors_in_64k);
+            next_sector += sectors_in_64k;
+        }
+        gcs().send_text(MAV_SEVERITY_WARNING, "Log corruption recovery complete, erased %d blocks", unsigned(blocks_erased));
+        df_EraseFrom = 0;
     }
 
     if (!CardInserted() || !log_write_started) {

libraries/AP_Logger/AP_Logger_Block.h

@@ -42,7 +42,8 @@ private:
      */
     virtual void BufferToPage(uint32_t PageAdr) = 0;
     virtual void PageToBuffer(uint32_t PageAdr) = 0;
-    virtual void SectorErase(uint32_t PageAdr) = 0;
+    virtual void SectorErase(uint32_t SectorAdr) = 0;
+    virtual void Sector4kErase(uint32_t SectorAdr) = 0;
     virtual void StartErase() = 0;
     virtual bool InErase() = 0;
 
@@ -59,7 +60,10 @@ private:
     uint32_t df_PageAdr;
     uint32_t df_Read_PageAdr;
     uint16_t df_FileNumber;
+    // relative page index of the current file starting at 1
     uint32_t df_FilePage;
+    // page to wipe from in the case of corruption
+    uint32_t df_EraseFrom;
 
     // offset from adding FMT messages to log data
     bool adding_fmt_headers;
@@ -74,6 +78,7 @@ private:
 
     // erase handling
     bool NeedErase(void);
+    void validate_log_structure();
 
     // internal high level functions
     int16_t get_log_data_raw(uint16_t log_num, uint32_t page, uint32_t offset, uint16_t len, uint8_t *data) WARN_IF_UNUSED;
@@ -97,12 +102,21 @@ private:
     void io_timer(void);
 
 protected:
-    // page handling
+    // number of bytes in a page
     uint32_t df_PageSize;
+    // number of pages in a (generally 64k) block
+    uint16_t df_PagePerBlock;
+    // number of pages in a (generally 4k) sector
     uint16_t df_PagePerSector;
+    // number of pages on the chip
     uint32_t df_NumPages;
     bool log_write_started;
 
+    // get the next sector from the current page
+    uint32_t get_sector(uint32_t current_page) {
+        return ((current_page - 1) / df_PagePerSector);
+    }
+
     static const uint16_t page_size_max = 256;
     uint8_t *buffer;
 

libraries/AP_Logger/AP_Logger_DataFlash.cpp

@@ -111,35 +111,40 @@ bool AP_Logger_DataFlash::getSectorCount(void)
 
     uint32_t id = buffer[0] << 16 | buffer[1] << 8 | buffer[2];
 
-    uint32_t sectors = 0;
+    uint32_t blocks = 0;
 
     switch (id) {
     case JEDEC_ID_WINBOND_W25Q16:
     case JEDEC_ID_MICRON_M25P16:
-        sectors = 32;
-        df_PagePerSector = 256;
+        blocks = 32;
+        df_PagePerBlock = 256;
+        df_PagePerSector = 16;
         break;
     case JEDEC_ID_WINBOND_W25Q32:
     case JEDEC_ID_MACRONIX_MX25L3206E:
-        sectors = 64;
-        df_PagePerSector = 256;
+        blocks = 64;
+        df_PagePerBlock = 256;
+        df_PagePerSector = 16;
         break;
     case JEDEC_ID_MICRON_N25Q064:
     case JEDEC_ID_WINBOND_W25Q64:
     case JEDEC_ID_MACRONIX_MX25L6406E:
-        sectors = 128;
-        df_PagePerSector = 256;
+        blocks = 128;
+        df_PagePerBlock = 256;
+        df_PagePerSector = 16;
         break;
     case JEDEC_ID_MICRON_N25Q128:
     case JEDEC_ID_WINBOND_W25Q128:
     case JEDEC_ID_CYPRESS_S25FL128L:
-        sectors = 256;
-        df_PagePerSector = 256;
+        blocks = 256;
+        df_PagePerBlock = 256;
+        df_PagePerSector = 16;
         break;
     case JEDEC_ID_WINBOND_W25Q256:
     case JEDEC_ID_MACRONIX_MX25L25635E:
-        sectors = 512;
-        df_PagePerSector = 256;
+        blocks = 512;
+        df_PagePerBlock = 256;
+        df_PagePerSector = 16;
         use_32bit_address = true;
         break;
     default:
@@ -149,12 +154,12 @@ bool AP_Logger_DataFlash::getSectorCount(void)
     }
 
     df_PageSize = 256;
-    df_NumPages = sectors * df_PagePerSector;
+    df_NumPages = blocks * df_PagePerBlock;
     erase_cmd = JEDEC_BLOCK64_ERASE;
 
     hal.scheduler->delay(2000);
     printf("SPI Flash 0x%08x found pages=%u erase=%uk\n",
-           id, df_NumPages, (df_PagePerSector * (uint32_t)df_PageSize)/1024);
+           id, df_NumPages, (df_PagePerBlock * (uint32_t)df_PageSize)/1024);
     return true;
 
 }
@@ -171,11 +176,7 @@ uint8_t AP_Logger_DataFlash::ReadStatusReg()
 
 bool AP_Logger_DataFlash::Busy()
 {
-    int32_t status = ReadStatusReg();
-    if (status < 0) {
-        return true;
-    }
-    return (status & JEDEC_STATUS_BUSY) != 0;
+    return (ReadStatusReg() & (JEDEC_STATUS_BUSY | JEDEC_STATUS_SRP0)) != 0;
 }
 
 void AP_Logger_DataFlash::Enter4ByteAddressMode(void)
@@ -247,16 +248,27 @@ void AP_Logger_DataFlash::BufferToPage(uint32_t pageNum)
 /*
   erase one sector (sizes varies with hw)
 */
-void AP_Logger_DataFlash::SectorErase(uint32_t sectorNum)
+void AP_Logger_DataFlash::SectorErase(uint32_t blockNum)
 {
     WriteEnable();
 
     WITH_SEMAPHORE(dev_sem);
 
-    uint32_t PageAdr = sectorNum * df_PageSize * df_PagePerSector;
+    uint32_t PageAdr = blockNum * df_PageSize * df_PagePerBlock;
     send_command_addr(erase_cmd, PageAdr);
 }
 
+/*
+  erase one 4k sector
+*/
+void AP_Logger_DataFlash::Sector4kErase(uint32_t sectorNum)
+{
+    WriteEnable();
+
+    WITH_SEMAPHORE(dev_sem);
+    uint32_t SectorAddr = sectorNum * df_PageSize * df_PagePerSector;
+    send_command_addr(JEDEC_SECTOR4_ERASE, SectorAddr);
+}
 
 void AP_Logger_DataFlash::StartErase()
 {
@@ -280,7 +292,6 @@ bool AP_Logger_DataFlash::InErase()
     return erase_start_ms != 0;
 }
 
-
 void AP_Logger_DataFlash::WriteEnable(void)
 {
     WaitReady();
@@ -293,8 +304,8 @@ void AP_Logger_DataFlash::flash_test()
 {
     for (uint8_t i=1; i<=20; i++) {
         printf("Flash fill %u\n", i);
-        if (i % df_PagePerSector == 0) {
-            SectorErase(i / df_PagePerSector);
+        if (i % df_PagePerBlock == 0) {
+            SectorErase(i / df_PagePerBlock);
         }
         memset(buffer, i, df_PageSize);
         BufferToPage(i);

libraries/AP_Logger/AP_Logger_DataFlash.h

@@ -21,6 +21,7 @@ private:
     void              BufferToPage(uint32_t PageAdr) override;
     void              PageToBuffer(uint32_t PageAdr) override;
     void              SectorErase(uint32_t SectorAdr) override;
+    void              Sector4kErase(uint32_t SectorAdr) override;
     void              StartErase() override;
     bool              InErase() override;
     void              send_command_addr(uint8_t cmd, uint32_t address);

libraries/AP_Logger/AP_Logger_MAVLinkLogTransfer.cpp

@@ -77,6 +77,8 @@ void AP_Logger::handle_log_request_list(GCS_MAVLINK &link, const mavlink_message
     mavlink_msg_log_request_list_decode(&msg, &packet);
 
     _log_num_logs = get_num_logs();
+    uint16_t last_log = find_last_log();
+
     if (_log_num_logs == 0) {
         _log_next_list_entry = 0;
         _log_last_list_entry = 0;        
@@ -84,11 +86,11 @@ void AP_Logger::handle_log_request_list(GCS_MAVLINK &link, const mavlink_message
         _log_next_list_entry = packet.start;
         _log_last_list_entry = packet.end;
 
-        if (_log_last_list_entry > _log_num_logs) {
-            _log_last_list_entry = _log_num_logs;
+        if (_log_last_list_entry > last_log) {
+            _log_last_list_entry = last_log;
         }
         if (_log_next_list_entry < 1) {
-            _log_next_list_entry = 1;
+            _log_next_list_entry = last_log - _log_num_logs + 1;
         }
     }
 
@@ -124,7 +126,8 @@ void AP_Logger::handle_log_request_data(GCS_MAVLINK &link, const mavlink_message
     if (transfer_activity != TransferActivity::SENDING || _log_num_data != packet.id) {
 
         uint16_t num_logs = get_num_logs();
-        if (packet.id > num_logs || packet.id < 1) {
+        uint16_t last_log = find_last_log();
+        if (packet.id > last_log || packet.id < (last_log - num_logs + 1)) {
             // request for an invalid log; cancel any current download
             transfer_activity = TransferActivity::IDLE;
             return;

libraries/AP_Logger/AP_Logger_SITL.cpp

@@ -40,6 +40,7 @@ void AP_Logger_SITL::Init()
 
     df_PageSize = DF_PAGE_SIZE;
     df_PagePerSector = DF_PAGE_PER_SECTOR;
+    df_PagePerBlock = DF_PAGE_PER_SECTOR;
     df_NumPages = DF_NUM_PAGES;
 
     AP_Logger_Block::Init();
@@ -75,6 +76,11 @@ void AP_Logger_SITL::SectorErase(uint32_t SectorAdr)
     }
 }
 
+void AP_Logger_SITL::Sector4kErase(uint32_t SectorAdr)
+{
+    SectorErase(SectorAdr);
+}
+
 void AP_Logger_SITL::StartErase()
 {
     for (uint32_t i=0; i<DF_NUM_PAGES/DF_PAGE_PER_SECTOR; i++) {

libraries/AP_Logger/AP_Logger_SITL.h

@@ -22,6 +22,7 @@ private:
     void  BufferToPage(uint32_t PageAdr) override;
     void  PageToBuffer(uint32_t PageAdr) override;
     void  SectorErase(uint32_t SectorAdr) override;
+    void  Sector4kErase(uint32_t SectorAdr) override;
     void  StartErase() override;
     bool  InErase() override;