From 7c57e1521cfed2f0ea18798f1daa2702c47eace4 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sun, 7 Nov 2021 19:30:47 +1100
Subject: [PATCH] AP_Filesystem: support formatting of SDcard on ChibiOS

---
 libraries/AP_Filesystem/AP_Filesystem.cpp     | 15 ++++++
 libraries/AP_Filesystem/AP_Filesystem.h       | 14 ++++++
 .../AP_Filesystem/AP_Filesystem_FATFS.cpp     | 47 +++++++++++++++++++
 libraries/AP_Filesystem/AP_Filesystem_FATFS.h |  7 +++
 .../AP_Filesystem/AP_Filesystem_backend.h     |  3 ++
 5 files changed, 86 insertions(+)

diff --git a/libraries/AP_Filesystem/AP_Filesystem.cpp b/libraries/AP_Filesystem/AP_Filesystem.cpp
index 279d1fc069..3be649a05d 100644
--- a/libraries/AP_Filesystem/AP_Filesystem.cpp
+++ b/libraries/AP_Filesystem/AP_Filesystem.cpp
@@ -70,6 +70,8 @@ const AP_Filesystem::Backend AP_Filesystem::backends[] = {
 #endif
 };
 
+extern const AP_HAL::HAL& hal;
+
 #define MAX_FD_PER_BACKEND 256U
 #define NUM_BACKENDS ARRAY_SIZE(backends)
 #define LOCAL_BACKEND backends[0]
@@ -269,6 +271,19 @@ bool AP_Filesystem::fgets(char *buf, uint8_t buflen, int fd)
     return i != 0;
 }
 
+// format filesystem
+bool AP_Filesystem::format(void)
+{
+#if AP_FILESYSTEM_FORMAT_ENABLED
+    if (hal.util->get_soft_armed()) {
+        return false;
+    }
+    return LOCAL_BACKEND.fs.format();
+#else
+    return false;
+#endif
+}
+
 namespace AP
 {
 AP_Filesystem &FS()
diff --git a/libraries/AP_Filesystem/AP_Filesystem.h b/libraries/AP_Filesystem/AP_Filesystem.h
index f42562a73d..5f886097e4 100644
--- a/libraries/AP_Filesystem/AP_Filesystem.h
+++ b/libraries/AP_Filesystem/AP_Filesystem.h
@@ -42,6 +42,13 @@ struct dirent {
 #include <fcntl.h>
 #include <errno.h>
 #include <unistd.h>
+
+#ifndef AP_FILESYSTEM_FORMAT_ENABLED
+// only enable for SDMMC filesystems for now as other types can't query
+// block size
+#define AP_FILESYSTEM_FORMAT_ENABLED (STM32_SDC_USE_SDMMC1==TRUE || STM32_SDC_USE_SDMMC2==TRUE)
+#endif
+
 #endif // HAL_BOARD_CHIBIOS
 #if CONFIG_HAL_BOARD == HAL_BOARD_LINUX || CONFIG_HAL_BOARD == HAL_BOARD_SITL
 #include "AP_Filesystem_posix.h"
@@ -53,6 +60,10 @@ struct dirent {
 
 #include "AP_Filesystem_backend.h"
 
+#ifndef AP_FILESYSTEM_FORMAT_ENABLED
+#define AP_FILESYSTEM_FORMAT_ENABLED 0
+#endif
+
 class AP_Filesystem {
 private:
     struct DirHandle {
@@ -96,6 +107,9 @@ public:
     // returns null-terminated string; cr or lf terminates line
     bool fgets(char *buf, uint8_t buflen, int fd);
 
+    // format filesystem
+    bool format(void);
+    
     /*
       load a full file. Use delete to free the data
      */
diff --git a/libraries/AP_Filesystem/AP_Filesystem_FATFS.cpp b/libraries/AP_Filesystem/AP_Filesystem_FATFS.cpp
index 7d4a254d8d..de19bbe960 100644
--- a/libraries/AP_Filesystem/AP_Filesystem_FATFS.cpp
+++ b/libraries/AP_Filesystem/AP_Filesystem_FATFS.cpp
@@ -10,6 +10,7 @@
 #if HAVE_FILESYSTEM_SUPPORT && CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
 
 #include <AP_HAL_ChibiOS/sdcard.h>
+#include <GCS_MAVLink/GCS.h>
 
 #if 0
 #define debug(fmt, args ...)  do {printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
@@ -851,6 +852,52 @@ void AP_Filesystem_FATFS::unmount(void)
     return sdcard_stop();
 }
 
+/*
+  format sdcard
+*/
+bool AP_Filesystem_FATFS::format(void)
+{
+#if FF_USE_MKFS
+    WITH_SEMAPHORE(sem);
+    hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_Filesystem_FATFS::format_handler, void));
+    // the format is handled asyncronously, we inform user of success
+    // via a text message
+    format_pending = true;
+    return true;
+#else
+    return false;
+#endif
+}
+
+/*
+  format sdcard
+*/
+void AP_Filesystem_FATFS::format_handler(void)
+{
+#if FF_USE_MKFS
+    if (!format_pending) {
+        return;
+    }
+    WITH_SEMAPHORE(sem);
+    format_pending = false;
+    GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Formatting SDCard");
+    uint8_t *buf = (uint8_t *)hal.util->malloc_type(FF_MAX_SS, AP_HAL::Util::MEM_DMA_SAFE);
+    if (buf == nullptr) {
+        return;
+    }
+    // format first disk
+    auto ret = f_mkfs("0:", 0, buf, FF_MAX_SS);
+    hal.util->free_type(buf, FF_MAX_SS, AP_HAL::Util::MEM_DMA_SAFE);
+    if (ret == FR_OK) {
+        GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Format: OK");
+    } else {
+        GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Format: Failed (%d)", int(ret));
+    }
+    sdcard_stop();
+    sdcard_retry();
+#endif
+}
+
 /*
   convert POSIX errno to text with user message.
 */
diff --git a/libraries/AP_Filesystem/AP_Filesystem_FATFS.h b/libraries/AP_Filesystem/AP_Filesystem_FATFS.h
index c21e4c9dd4..cabb3ba6e0 100644
--- a/libraries/AP_Filesystem/AP_Filesystem_FATFS.h
+++ b/libraries/AP_Filesystem/AP_Filesystem_FATFS.h
@@ -54,4 +54,11 @@ public:
 
     // unmount filesystem for reboot
     void unmount(void) override;
+
+    // format sdcard
+    bool format(void) override;
+
+private:
+    void format_handler(void);
+    bool format_pending;
 };
diff --git a/libraries/AP_Filesystem/AP_Filesystem_backend.h b/libraries/AP_Filesystem/AP_Filesystem_backend.h
index 5c37298d89..a60fad1d17 100644
--- a/libraries/AP_Filesystem/AP_Filesystem_backend.h
+++ b/libraries/AP_Filesystem/AP_Filesystem_backend.h
@@ -73,6 +73,9 @@ public:
     // unmount filesystem for reboot
     virtual void unmount(void) {}
 
+    // format sdcard
+    virtual bool format(void) { return false; }
+    
     /*
       load a full file. Use delete to free the data
      */