diff --git a/libraries/AP_Filesystem/AP_Filesystem.cpp b/libraries/AP_Filesystem/AP_Filesystem.cpp
new file mode 100644
index 0000000000..fb7b816f3f
--- /dev/null
+++ b/libraries/AP_Filesystem/AP_Filesystem.cpp
@@ -0,0 +1,29 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+
+#include "AP_Filesystem.h"
+
+#if HAVE_FILESYSTEM_SUPPORT
+
+static AP_Filesystem fs;
+
+namespace AP
+{
+AP_Filesystem &FS()
+{
+ return fs;
+}
+}
+#endif
diff --git a/libraries/AP_Filesystem/AP_Filesystem.h b/libraries/AP_Filesystem/AP_Filesystem.h
new file mode 100644
index 0000000000..97a3934551
--- /dev/null
+++ b/libraries/AP_Filesystem/AP_Filesystem.h
@@ -0,0 +1,68 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+/*
+ ArduPilot filesystem interface. This offsets a minimal subset of
+ full functionality offered by posix type interfaces, meeting the
+ needs of ArduPilot
+ */
+#pragma once
+
+#include
+#include
+
+#if HAL_OS_POSIX_IO || HAL_OS_FATFS_IO
+#define HAVE_FILESYSTEM_SUPPORT 1
+#else
+#define HAVE_FILESYSTEM_SUPPORT 0
+#endif
+
+#if HAVE_FILESYSTEM_SUPPORT
+#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
+#include "AP_Filesystem_FATFS.h"
+#endif
+#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX || CONFIG_HAL_BOARD == HAL_BOARD_SITL
+#include "AP_Filesystem_posix.h"
+#endif
+
+class AP_Filesystem {
+
+public:
+ AP_Filesystem() {}
+
+ // functions that closely match the equivalent posix calls
+ int open(const char *fname, int flags);
+ int close(int fd);
+ ssize_t read(int fd, void *buf, size_t count);
+ ssize_t write(int fd, const void *buf, size_t count);
+ int fsync(int fd);
+ off_t lseek(int fd, off_t offset, int whence);
+ int stat(const char *pathname, struct stat *stbuf);
+ int unlink(const char *pathname);
+ int mkdir(const char *pathname);
+ DIR *opendir(const char *pathname);
+ struct dirent *readdir(DIR *dirp);
+ int closedir(DIR *dirp);
+
+ // return free disk space in bytes, -1 on error
+ int64_t disk_free(const char *path);
+
+ // return total disk space in bytes, -1 on error
+ int64_t disk_space(const char *path);
+};
+
+namespace AP {
+ AP_Filesystem &FS();
+};
+#endif // HAVE_FILESYSTEM_SUPPORT
diff --git a/libraries/AP_Filesystem/AP_Filesystem_FATFS.cpp b/libraries/AP_Filesystem/AP_Filesystem_FATFS.cpp
new file mode 100644
index 0000000000..ac481d3011
--- /dev/null
+++ b/libraries/AP_Filesystem/AP_Filesystem_FATFS.cpp
@@ -0,0 +1,833 @@
+/*
+ ArduPilot filesystem interface for systems using the FATFS filesystem
+ */
+#include "AP_Filesystem.h"
+#include
+#include
+
+#if HAVE_FILESYSTEM_SUPPORT && CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
+
+#include
+
+#if 0
+#define debug(fmt, args ...) do {printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)
+#else
+#define debug(fmt, args ...)
+#endif
+
+extern const AP_HAL::HAL& hal;
+
+static bool remount_needed;
+
+#define FATFS_R (S_IRUSR | S_IRGRP | S_IROTH) /*< FatFs Read perms */
+#define FATFS_W (S_IWUSR | S_IWGRP | S_IWOTH) /*< FatFs Write perms */
+#define FATFS_X (S_IXUSR | S_IXGRP | S_IXOTH) /*< FatFs Execute perms */
+
+// use a semaphore to ensure that only one filesystem operation is
+// happening at a time
+static HAL_Semaphore sem;
+
+typedef struct {
+ FIL *fh;
+ char *name;
+} FAT_FILE;
+
+#define MAX_FILES 16
+static FAT_FILE *file_table[MAX_FILES];
+
+static bool isatty(int fileno)
+{
+ if (fileno >= 0 && fileno <= 2) {
+ return true;
+ }
+ return false;
+}
+
+/*
+ allocate a file descriptor
+*/
+static int new_file_descriptor(const char *pathname)
+{
+ int i;
+ FAT_FILE *stream;
+ FIL *fh;
+
+ for (i=0; iname = fname;
+
+ file_table[i] = stream;
+ stream->fh = fh;
+ return i;
+ }
+ }
+ errno = ENFILE;
+ return -1;
+}
+
+static FAT_FILE *fileno_to_stream(int fileno)
+{
+ FAT_FILE *stream;
+ if (fileno < 0 || fileno >= MAX_FILES) {
+ errno = EBADF;
+ return nullptr;
+ }
+
+ stream = file_table[fileno];
+ if (stream == NULL) {
+ errno = EBADF;
+ return nullptr;
+ }
+ return stream;
+}
+
+static int free_file_descriptor(int fileno)
+{
+ FAT_FILE *stream;
+ FIL *fh;
+
+ if (isatty( fileno )) {
+ errno = EBADF;
+ return -1;
+ }
+
+ // checks if fileno out of bounds
+ stream = fileno_to_stream(fileno);
+ if (stream == NULL) {
+ return -1;
+ }
+
+ fh = stream->fh;
+
+ if (fh != NULL) {
+ free(fh);
+ }
+
+ free(stream->name);
+ stream->name = NULL;
+
+ file_table[fileno] = NULL;
+ free(stream);
+ return fileno;
+}
+
+static FIL *fileno_to_fatfs(int fileno)
+{
+ FAT_FILE *stream;
+ FIL *fh;
+
+ if (isatty( fileno )) {
+ errno = EBADF;
+ return nullptr;
+ }
+
+ // checks if fileno out of bounds
+ stream = fileno_to_stream(fileno);
+ if ( stream == NULL ) {
+ return nullptr;
+ }
+
+ fh = stream->fh;
+ if (fh == NULL) {
+ errno = EBADF;
+ return nullptr;
+ }
+ return fh;
+}
+
+static int fatfs_to_errno( FRESULT Result )
+{
+ switch ( Result ) {
+ case FR_OK: /* FatFS (0) Succeeded */
+ return 0; /* POSIX OK */
+ case FR_DISK_ERR: /* FatFS (1) A hard error occurred in the low level disk I/O layer */
+ return EIO; /* POSIX Input/output error (POSIX.1) */
+
+ case FR_INT_ERR: /* FatFS (2) Assertion failed */
+ return EPERM; /* POSIX Operation not permitted (POSIX.1) */
+
+ case FR_NOT_READY: /* FatFS (3) The physical drive cannot work */
+ return EBUSY; /* POSIX Device or resource busy (POSIX.1) */
+
+ case FR_NO_FILE: /* FatFS (4) Could not find the file */
+ return ENOENT; /* POSIX No such file or directory (POSIX.1) */
+
+ case FR_NO_PATH: /* FatFS (5) Could not find the path */
+ return ENOENT; /* POSIX No such file or directory (POSIX.1) */
+
+ case FR_INVALID_NAME: /* FatFS (6) The path name format is invalid */
+ return EINVAL; /* POSIX Invalid argument (POSIX.1) */
+
+ case FR_DENIED: /* FatFS (7) Access denied due to prohibited access or directory full */
+ return EACCES; /* POSIX Permission denied (POSIX.1) */
+
+ case FR_EXIST: /* file exists */
+ return EEXIST; /* file exists */
+
+ case FR_INVALID_OBJECT: /* FatFS (9) The file/directory object is invalid */
+ return EINVAL; /* POSIX Invalid argument (POSIX.1) */
+
+ case FR_WRITE_PROTECTED: /* FatFS (10) The physical drive is write protected */
+ return EROFS; /* POSIX Read-only filesystem (POSIX.1) */
+
+ case FR_INVALID_DRIVE: /* FatFS (11) The logical drive number is invalid */
+ return ENXIO; /* POSIX No such device or address (POSIX.1) */
+
+ case FR_NOT_ENABLED: /* FatFS (12) The volume has no work area */
+ return ENOSPC; /* POSIX No space left on device (POSIX.1) */
+
+ case FR_NO_FILESYSTEM: /* FatFS (13) There is no valid FAT volume */
+ return ENXIO; /* POSIX No such device or address (POSIX.1) */
+
+ case FR_MKFS_ABORTED: /* FatFS (14) The f_mkfs() aborted due to any parameter error */
+ return EINVAL; /* POSIX Invalid argument (POSIX.1) */
+
+ case FR_TIMEOUT: /* FatFS (15) Could not get a grant to access the volume within defined period */
+ return EBUSY; /* POSIX Device or resource busy (POSIX.1) */
+
+ case FR_LOCKED: /* FatFS (16) The operation is rejected according to the file sharing policy */
+ return EBUSY; /* POSIX Device or resource busy (POSIX.1) */
+
+
+ case FR_NOT_ENOUGH_CORE: /* FatFS (17) LFN working buffer could not be allocated */
+ return ENOMEM; /* POSIX Not enough space (POSIX.1) */
+
+ case FR_TOO_MANY_OPEN_FILES:/* FatFS (18) Number of open files > _FS_SHARE */
+ return EMFILE; /* POSIX Too many open files (POSIX.1) */
+
+ case FR_INVALID_PARAMETER:/* FatFS (19) Given parameter is invalid */
+ return EINVAL; /* POSIX Invalid argument (POSIX.1) */
+
+ }
+ return EBADMSG; /* POSIX Bad message (POSIX.1) */
+}
+
+// check for a remount and return -1 if it fails
+#define CHECK_REMOUNT() do { if (remount_needed && !remount_file_system()) { errno = EIO; return -1; }} while (0)
+#define CHECK_REMOUNT_NULL() do { if (remount_needed && !remount_file_system()) { errno = EIO; return NULL; }} while (0)
+
+// we allow for IO retries if either not armed or not in main thread
+#define RETRY_ALLOWED() (!hal.scheduler->in_main_thread() || !hal.util->get_soft_armed())
+
+/*
+ try to remount the file system on disk error
+ */
+static bool remount_file_system(void)
+{
+ EXPECT_DELAY_MS(3000);
+ if (!remount_needed) {
+ sdcard_stop();
+ }
+ if (!sdcard_retry()) {
+ remount_needed = true;
+ EXPECT_DELAY_MS(0);
+ return false;
+ }
+ remount_needed = false;
+ for (uint16_t i=0; ifh;
+ FSIZE_t offset = fh->fptr;
+ uint8_t flags = fh->flag & (FA_READ | FA_WRITE);
+
+ memset(fh, 0, sizeof(*fh));
+ if (flags & FA_WRITE) {
+ // the file may not have been created yet on the sdcard
+ flags |= FA_OPEN_ALWAYS;
+ }
+ FRESULT res = f_open(fh, f->name, flags);
+ debug("reopen %s flags=0x%x ofs=%u -> %d\n", f->name, unsigned(flags), unsigned(offset), int(res));
+ if (res == FR_OK) {
+ f_lseek(fh, offset);
+ }
+ }
+ EXPECT_DELAY_MS(0);
+ return true;
+}
+
+int AP_Filesystem::open(const char *pathname, int flags)
+{
+ int fileno;
+ int fatfs_modes;
+ FAT_FILE *stream;
+ FIL *fh;
+ int res;
+
+ WITH_SEMAPHORE(sem);
+
+ CHECK_REMOUNT();
+
+ errno = 0;
+ debug("Open %s 0x%x", pathname, flags);
+
+ if ((flags & O_ACCMODE) == O_RDWR) {
+ fatfs_modes = FA_READ | FA_WRITE;
+ } else if ((flags & O_ACCMODE) == O_RDONLY) {
+ fatfs_modes = FA_READ;
+ } else {
+ fatfs_modes = FA_WRITE;
+ }
+
+ if (flags & O_CREAT) {
+ if (flags & O_TRUNC) {
+ fatfs_modes |= FA_CREATE_ALWAYS;
+ } else {
+ fatfs_modes |= FA_OPEN_ALWAYS;
+ }
+ }
+
+ fileno = new_file_descriptor(pathname);
+
+ // checks if fileno out of bounds
+ stream = fileno_to_stream(fileno);
+ if (stream == NULL) {
+ free_file_descriptor(fileno);
+ return -1;
+ }
+
+ // fileno_to_fatfs checks for fileno out of bounds
+ fh = fileno_to_fatfs(fileno);
+ if (fh == NULL) {
+ free_file_descriptor(fileno);
+ errno = EBADF;
+ return -1;
+ }
+ res = f_open(fh, pathname, (BYTE) (fatfs_modes & 0xff));
+ if (res == FR_DISK_ERR && RETRY_ALLOWED()) {
+ // one retry on disk error
+ hal.scheduler->delay(100);
+ if (remount_file_system()) {
+ res = f_open(fh, pathname, (BYTE) (fatfs_modes & 0xff));
+ }
+ }
+ if (res != FR_OK) {
+ errno = fatfs_to_errno((FRESULT)res);
+ free_file_descriptor(fileno);
+ return -1;
+ }
+ if (flags & O_APPEND) {
+ /// Seek to end of the file
+ res = f_lseek(fh, f_size(fh));
+ if (res != FR_OK) {
+ errno = fatfs_to_errno((FRESULT)res);
+ f_close(fh);
+ free_file_descriptor(fileno);
+ return -1;
+ }
+ }
+
+ debug("Open %s -> %d", pathname, fileno);
+
+ return fileno;
+}
+
+int AP_Filesystem::close(int fileno)
+{
+ FAT_FILE *stream;
+ FIL *fh;
+ int res;
+
+ WITH_SEMAPHORE(sem);
+
+ errno = 0;
+
+ // checks if fileno out of bounds
+ stream = fileno_to_stream(fileno);
+ if (stream == NULL) {
+ return -1;
+ }
+
+ // fileno_to_fatfs checks for fileno out of bounds
+ fh = fileno_to_fatfs(fileno);
+ if (fh == NULL) {
+ return -1;
+ }
+ res = f_close(fh);
+ free_file_descriptor(fileno);
+ if (res != FR_OK) {
+ errno = fatfs_to_errno((FRESULT)res);
+ return -1;
+ }
+ return 0;
+}
+
+ssize_t AP_Filesystem::read(int fd, void *buf, size_t count)
+{
+ UINT size;
+ UINT bytes = count;
+ int res;
+ FIL *fh;
+
+ WITH_SEMAPHORE(sem);
+
+ CHECK_REMOUNT();
+
+ if (count > 0) {
+ *(char *) buf = 0;
+ }
+
+ errno = 0;
+
+ // fileno_to_fatfs checks for fd out of bounds
+ fh = fileno_to_fatfs(fd);
+ if ( fh == NULL ) {
+ errno = EBADF;
+ return -1;
+ }
+
+ res = f_read(fh, (void *) buf, bytes, &size);
+ if (res != FR_OK) {
+ errno = fatfs_to_errno((FRESULT)res);
+ return -1;
+ }
+ return (ssize_t)size;
+}
+
+ssize_t AP_Filesystem::write(int fd, const void *buf, size_t count)
+{
+ UINT size;
+ UINT bytes = count;
+ FRESULT res;
+ FIL *fh;
+ errno = 0;
+
+ WITH_SEMAPHORE(sem);
+
+ CHECK_REMOUNT();
+
+ // fileno_to_fatfs checks for fd out of bounds
+ fh = fileno_to_fatfs(fd);
+ if ( fh == NULL ) {
+ errno = EBADF;
+ return -1;
+ }
+
+ res = f_write(fh, buf, bytes, &size);
+ if (res == FR_DISK_ERR && RETRY_ALLOWED()) {
+ // one retry on disk error
+ hal.scheduler->delay(100);
+ if (remount_file_system()) {
+ res = f_write(fh, buf, bytes, &size);
+ }
+ }
+ if (res != FR_OK) {
+ errno = fatfs_to_errno(res);
+ return -1;
+ }
+ return (ssize_t)size;
+}
+
+int AP_Filesystem::fsync(int fileno)
+{
+ FAT_FILE *stream;
+ FIL *fh;
+ int res;
+
+ WITH_SEMAPHORE(sem);
+
+ errno = 0;
+
+ // checks if fileno out of bounds
+ stream = fileno_to_stream(fileno);
+ if (stream == NULL) {
+ return -1;
+ }
+
+ // fileno_to_fatfs checks for fileno out of bounds
+ fh = fileno_to_fatfs(fileno);
+ if (fh == NULL) {
+ return -1;
+ }
+ res = f_sync(fh);
+ if (res != FR_OK) {
+ errno = fatfs_to_errno((FRESULT)res);
+ return -1;
+ }
+ return 0;
+}
+
+off_t AP_Filesystem::lseek(int fileno, off_t position, int whence)
+{
+ FRESULT res;
+ FIL *fh;
+ errno = 0;
+
+ WITH_SEMAPHORE(sem);
+
+ // fileno_to_fatfs checks for fd out of bounds
+ fh = fileno_to_fatfs(fileno);
+ if (fh == NULL) {
+ errno = EMFILE;
+ return -1;
+ }
+ if (isatty(fileno)) {
+ return -1;
+ }
+
+ if (whence == SEEK_END) {
+ position += f_size(fh);
+ } else if (whence==SEEK_CUR) {
+ position += fh->fptr;
+ }
+
+ res = f_lseek(fh, position);
+ if (res) {
+ errno = fatfs_to_errno(res);
+ return -1;
+ }
+ return fh->fptr;
+}
+
+/*
+ mktime replacement from Samba
+ */
+static time_t replace_mktime(const struct tm *t)
+{
+ time_t epoch = 0;
+ int n;
+ int mon [] = { 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 }, y, m, i;
+ const unsigned MINUTE = 60;
+ const unsigned HOUR = 60*MINUTE;
+ const unsigned DAY = 24*HOUR;
+ const unsigned YEAR = 365*DAY;
+
+ if (t->tm_year < 70) {
+ return (time_t)-1;
+ }
+
+ n = t->tm_year + 1900 - 1;
+ epoch = (t->tm_year - 70) * YEAR +
+ ((n / 4 - n / 100 + n / 400) - (1969 / 4 - 1969 / 100 + 1969 / 400)) * DAY;
+
+ y = t->tm_year + 1900;
+ m = 0;
+
+ for (i = 0; i < t->tm_mon; i++) {
+ epoch += mon [m] * DAY;
+ if (m == 1 && y % 4 == 0 && (y % 100 != 0 || y % 400 == 0)) {
+ epoch += DAY;
+ }
+
+ if (++m > 11) {
+ m = 0;
+ y++;
+ }
+ }
+
+ epoch += (t->tm_mday - 1) * DAY;
+ epoch += t->tm_hour * HOUR + t->tm_min * MINUTE + t->tm_sec;
+
+ return epoch;
+}
+
+static time_t fat_time_to_unix(uint16_t date, uint16_t time)
+{
+ struct tm tp;
+ time_t unix;
+
+ memset(&tp, 0, sizeof(struct tm));
+
+ tp.tm_sec = (time << 1) & 0x3e; // 2 second resolution
+ tp.tm_min = ((time >> 5) & 0x3f);
+ tp.tm_hour = ((time >> 11) & 0x1f);
+ tp.tm_mday = (date & 0x1f);
+ tp.tm_mon = ((date >> 5) & 0x0f) - 1;
+ tp.tm_year = ((date >> 9) & 0x7f) + 80;
+ unix = replace_mktime( &tp );
+ return unix;
+}
+
+int AP_Filesystem::stat(const char *name, struct stat *buf)
+{
+ FILINFO info;
+ int res;
+ time_t epoch;
+ uint16_t mode;
+
+ WITH_SEMAPHORE(sem);
+
+ CHECK_REMOUNT();
+
+ errno = 0;
+
+ // f_stat does not handle / or . as root directory
+ if (strcmp(name,"/") == 0 || strcmp(name,".") == 0) {
+ buf->st_atime = 0;
+ buf->st_mtime = 0;
+ buf->st_ctime = 0;
+ buf->st_uid= 0;
+ buf->st_gid= 0;
+ buf->st_size = 0;
+ buf->st_mode = S_IFDIR;
+ return 0;
+ }
+
+ res = f_stat(name, &info);
+ if (res == FR_DISK_ERR && RETRY_ALLOWED()) {
+ // one retry on disk error
+ if (remount_file_system()) {
+ res = f_stat(name, &info);
+ }
+ }
+ if (res != FR_OK) {
+ errno = fatfs_to_errno((FRESULT)res);
+ return -1;
+ }
+
+ buf->st_size = info.fsize;
+ epoch = fat_time_to_unix(info.fdate, info.ftime);
+ buf->st_atime = epoch; // Access time
+ buf->st_mtime = epoch; // Modification time
+ buf->st_ctime = epoch; // Creation time
+
+ // We only handle read only case
+ mode = (FATFS_R | FATFS_X);
+ if ( !(info.fattrib & AM_RDO)) {
+ mode |= (FATFS_W); // enable write if NOT read only
+ }
+
+ if (info.fattrib & AM_SYS) {
+ buf->st_uid= 0;
+ buf->st_gid= 0;
+ }
+ {
+ buf->st_uid=1000;
+ buf->st_gid=1000;
+ }
+
+ if (info.fattrib & AM_DIR) {
+ mode |= S_IFDIR;
+ } else {
+ mode |= S_IFREG;
+ }
+ buf->st_mode = mode;
+
+ return 0;
+}
+
+int AP_Filesystem::unlink(const char *pathname)
+{
+ WITH_SEMAPHORE(sem);
+
+ errno = 0;
+ int res = f_unlink(pathname);
+ if (res != FR_OK) {
+ errno = fatfs_to_errno((FRESULT)res);
+ return -1;
+ }
+ return 0;
+}
+
+int AP_Filesystem::mkdir(const char *pathname)
+{
+ WITH_SEMAPHORE(sem);
+
+ errno = 0;
+
+ int res = f_mkdir(pathname);
+ if (res != FR_OK) {
+ errno = fatfs_to_errno((FRESULT)res);
+ return -1;
+ }
+
+ return 0;
+}
+
+/*
+ wrapper structure to associate a dirent with a DIR
+ */
+struct DIR_Wrapper {
+ DIR d; // must be first structure element
+ struct dirent de;
+};
+
+DIR *AP_Filesystem::opendir(const char *pathdir)
+{
+ WITH_SEMAPHORE(sem);
+
+ CHECK_REMOUNT_NULL();
+
+ debug("Opendir %s", pathdir);
+ struct DIR_Wrapper *ret = new DIR_Wrapper;
+ if (!ret) {
+ return nullptr;
+ }
+ int res = f_opendir(&ret->d, pathdir);
+ if (res == FR_DISK_ERR && RETRY_ALLOWED()) {
+ // one retry on disk error
+ if (remount_file_system()) {
+ res = f_opendir(&ret->d, pathdir);
+ }
+ }
+ if (res != FR_OK) {
+ errno = fatfs_to_errno((FRESULT)res);
+ delete ret;
+ return nullptr;
+ }
+ debug("Opendir %s -> %p", pathdir, ret);
+ return &ret->d;
+}
+
+struct dirent *AP_Filesystem::readdir(DIR *dirp)
+{
+ WITH_SEMAPHORE(sem);
+
+ struct DIR_Wrapper *d = (struct DIR_Wrapper *)dirp;
+ if (!d) {
+ errno = EINVAL;
+ return nullptr;
+ }
+ FILINFO fno;
+ int len;
+ int res;
+
+ d->de.d_name[0] = 0;
+ res = f_readdir ( dirp, &fno );
+ if (res != FR_OK || fno.fname[0] == 0) {
+ errno = fatfs_to_errno((FRESULT)res);
+ return nullptr;
+ }
+ len = strlen(fno.fname);
+ strncpy(d->de.d_name,fno.fname,len);
+ d->de.d_name[len] = 0;
+ return &d->de;
+}
+
+int AP_Filesystem::closedir(DIR *dirp)
+{
+ WITH_SEMAPHORE(sem);
+
+ struct DIR_Wrapper *d = (struct DIR_Wrapper *)dirp;
+ if (!d) {
+ errno = EINVAL;
+ return -1;
+ }
+ int res = f_closedir (dirp);
+ delete d;
+ if (res != FR_OK) {
+ errno = fatfs_to_errno((FRESULT)res);
+ return -1;
+ }
+ debug("closedir");
+ return 0;
+}
+
+// return free disk space in bytes
+int64_t AP_Filesystem::disk_free(const char *path)
+{
+ WITH_SEMAPHORE(sem);
+
+ FATFS *fs;
+ DWORD fre_clust, fre_sect;
+
+ CHECK_REMOUNT();
+
+ /* Get volume information and free clusters of drive 1 */
+ FRESULT res = f_getfree("/", &fre_clust, &fs);
+ if (res) {
+ return res;
+ }
+
+ /* Get total sectors and free sectors */
+ fre_sect = fre_clust * fs->csize;
+ return (int64_t)(fre_sect)*512;
+}
+
+// return total disk space in bytes
+int64_t AP_Filesystem::disk_space(const char *path)
+{
+ WITH_SEMAPHORE(sem);
+
+ CHECK_REMOUNT();
+
+ FATFS *fs;
+ DWORD fre_clust, tot_sect;
+
+ /* Get volume information and free clusters of drive 1 */
+ FRESULT res = f_getfree("/", &fre_clust, &fs);
+ if (res) {
+ return -1;
+ }
+
+ /* Get total sectors and free sectors */
+ tot_sect = (fs->n_fatent - 2) * fs->csize;
+ return (int64_t)(tot_sect)*512;
+}
+
+/*
+ convert POSIX errno to text with user message.
+*/
+char *strerror(int errnum)
+{
+#define SWITCH_ERROR(errno) case errno: return const_cast(#errno); break
+ switch (errnum) {
+ SWITCH_ERROR(EPERM);
+ SWITCH_ERROR(ENOENT);
+ SWITCH_ERROR(ESRCH);
+ SWITCH_ERROR(EINTR);
+ SWITCH_ERROR(EIO);
+ SWITCH_ERROR(ENXIO);
+ SWITCH_ERROR(E2BIG);
+ SWITCH_ERROR(ENOEXEC);
+ SWITCH_ERROR(EBADF);
+ SWITCH_ERROR(ECHILD);
+ SWITCH_ERROR(EAGAIN);
+ SWITCH_ERROR(ENOMEM);
+ SWITCH_ERROR(EACCES);
+ SWITCH_ERROR(EFAULT);
+#ifdef ENOTBLK
+ SWITCH_ERROR(ENOTBLK);
+#endif // ENOTBLK
+ SWITCH_ERROR(EBUSY);
+ SWITCH_ERROR(EEXIST);
+ SWITCH_ERROR(EXDEV);
+ SWITCH_ERROR(ENODEV);
+ SWITCH_ERROR(ENOTDIR);
+ SWITCH_ERROR(EISDIR);
+ SWITCH_ERROR(EINVAL);
+ SWITCH_ERROR(ENFILE);
+ SWITCH_ERROR(EMFILE);
+ SWITCH_ERROR(ENOTTY);
+ SWITCH_ERROR(ETXTBSY);
+ SWITCH_ERROR(EFBIG);
+ SWITCH_ERROR(ENOSPC);
+ SWITCH_ERROR(ESPIPE);
+ SWITCH_ERROR(EROFS);
+ SWITCH_ERROR(EMLINK);
+ SWITCH_ERROR(EPIPE);
+ SWITCH_ERROR(EDOM);
+ SWITCH_ERROR(ERANGE);
+ SWITCH_ERROR(EBADMSG);
+ }
+
+#undef SWITCH_ERROR
+
+ return NULL;
+}
+
+#endif // CONFIG_HAL_BOARD
diff --git a/libraries/AP_Filesystem/AP_Filesystem_FATFS.h b/libraries/AP_Filesystem/AP_Filesystem_FATFS.h
new file mode 100644
index 0000000000..e6fa13c804
--- /dev/null
+++ b/libraries/AP_Filesystem/AP_Filesystem_FATFS.h
@@ -0,0 +1,25 @@
+/*
+ FATFS backend for AP_Filesystem
+ */
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+// Seek offset macros
+#define SEEK_SET 0
+#define SEEK_CUR 1
+#define SEEK_END 2
+
+#if FF_USE_LFN != 0
+#define MAX_NAME_LEN FF_MAX_LFN
+#else
+#define MAX_NAME_LEN 13
+#endif
+
+struct dirent {
+ char d_name[MAX_NAME_LEN]; /* filename */
+};
diff --git a/libraries/AP_Filesystem/AP_Filesystem_posix.cpp b/libraries/AP_Filesystem/AP_Filesystem_posix.cpp
new file mode 100644
index 0000000000..a748f7c60c
--- /dev/null
+++ b/libraries/AP_Filesystem/AP_Filesystem_posix.cpp
@@ -0,0 +1,110 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+/*
+ ArduPilot filesystem interface for posix systems
+ */
+#include "AP_Filesystem.h"
+#include
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX
+
+#include
+
+extern const AP_HAL::HAL& hal;
+
+int AP_Filesystem::open(const char *fname, int flags)
+{
+ // we automatically add O_CLOEXEC as we always want it for ArduPilot FS usage
+ return ::open(fname, flags | O_CLOEXEC, 0644);
+}
+
+int AP_Filesystem::close(int fd)
+{
+ return ::close(fd);
+}
+
+ssize_t AP_Filesystem::read(int fd, void *buf, size_t count)
+{
+ return ::read(fd, buf, count);
+}
+
+ssize_t AP_Filesystem::write(int fd, const void *buf, size_t count)
+{
+ return ::write(fd, buf, count);
+}
+
+int AP_Filesystem::fsync(int fd)
+{
+ return ::fsync(fd);
+}
+
+off_t AP_Filesystem::lseek(int fd, off_t offset, int seek_from)
+{
+ return ::lseek(fd, offset, seek_from);
+}
+
+int AP_Filesystem::stat(const char *pathname, struct stat *stbuf)
+{
+ return ::stat(pathname, stbuf);
+}
+
+int AP_Filesystem::unlink(const char *pathname)
+{
+ return ::unlink(pathname);
+}
+
+int AP_Filesystem::mkdir(const char *pathname)
+{
+ return ::mkdir(pathname, 0775);
+}
+
+DIR *AP_Filesystem::opendir(const char *pathname)
+{
+ return ::opendir(pathname);
+}
+
+struct dirent *AP_Filesystem::readdir(DIR *dirp)
+{
+ return ::readdir(dirp);
+}
+
+int AP_Filesystem::closedir(DIR *dirp)
+{
+ return ::closedir(dirp);
+}
+
+// return free disk space in bytes
+int64_t AP_Filesystem::disk_free(const char *path)
+{
+ struct statfs stats;
+ if (::statfs(path, &stats) < 0) {
+ return -1;
+ }
+ return (((int64_t)stats.f_bavail) * stats.f_bsize);
+}
+
+// return total disk space in bytes
+int64_t AP_Filesystem::disk_space(const char *path)
+{
+ struct statfs stats;
+ if (::statfs(path, &stats) < 0) {
+ return -1;
+ }
+ return (((int64_t)stats.f_blocks) * stats.f_bsize);
+}
+
+
+#endif // CONFIG_HAL_BOARD
+
diff --git a/libraries/AP_Filesystem/AP_Filesystem_posix.h b/libraries/AP_Filesystem/AP_Filesystem_posix.h
new file mode 100644
index 0000000000..1f5f8bd2a8
--- /dev/null
+++ b/libraries/AP_Filesystem/AP_Filesystem_posix.h
@@ -0,0 +1,25 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+
+#pragma once
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
diff --git a/libraries/AP_Filesystem/posix_compat.cpp b/libraries/AP_Filesystem/posix_compat.cpp
new file mode 100644
index 0000000000..e4a3555849
--- /dev/null
+++ b/libraries/AP_Filesystem/posix_compat.cpp
@@ -0,0 +1,265 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+/*
+ compatibility with posix APIs using AP_Filesystem
+
+ This implements the FILE* API from posix sufficiently well for Lua
+ scripting to function. It has no buffering so is inefficient for
+ single character operations. We deliberately use this implementation
+ in HAL_SITL and HAL_Linux where it is not needed in order to have a
+ uniform implementation across all platforms
+ */
+
+#include "AP_Filesystem.h"
+
+#if HAVE_FILESYSTEM_SUPPORT
+
+#include "posix_compat.h"
+#include
+#include
+
+struct apfs_file {
+ int fd;
+ bool error;
+ bool eof;
+ int16_t unget;
+ char *tmpfile_name;
+};
+
+#define CHECK_STREAM(stream, ret) while (stream == NULL || stream->fd < 0) { errno = EBADF; return ret; }
+
+#define modecmp(str, pat) (strcmp(str, pat) == 0 ? 1: 0)
+
+/*
+ map a fopen() file mode to a open() mode
+ */
+static int posix_fopen_modes_to_open(const char *mode)
+{
+ int flag = 0;
+
+ if (modecmp(mode,"r") || modecmp(mode,"rb")) {
+ flag = O_RDONLY;
+ return flag;
+ }
+ if (modecmp(mode,"r+") || modecmp(mode, "r+b" ) || modecmp(mode, "rb+" )) {
+ flag = O_RDWR | O_TRUNC;
+ return flag;
+ }
+ if (modecmp(mode,"w") || modecmp(mode,"wb")) {
+ flag = O_WRONLY | O_CREAT | O_TRUNC;
+ return flag;
+ }
+ if (modecmp(mode,"w+") || modecmp(mode, "w+b" ) || modecmp(mode, "wb+" )) {
+ flag = O_RDWR | O_CREAT | O_TRUNC;
+ return flag;
+ }
+ if (modecmp(mode,"a") || modecmp(mode,"ab")) {
+ flag = O_WRONLY | O_CREAT | O_APPEND;
+ return flag;
+ }
+ if (modecmp(mode,"a+") || modecmp(mode, "a+b" ) || modecmp(mode, "ab+" )) {
+ flag = O_RDWR | O_CREAT | O_APPEND;
+ return -1;
+ }
+ return -1;
+}
+
+APFS_FILE *apfs_fopen(const char *pathname, const char *mode)
+{
+ APFS_FILE *f = new APFS_FILE;
+ if (!f) {
+ return nullptr;
+ }
+ f->fd = AP::FS().open(pathname, posix_fopen_modes_to_open(mode));
+ f->unget = -1;
+ return f;
+}
+
+int apfs_fprintf(APFS_FILE *stream, const char *fmt, ...)
+{
+ CHECK_STREAM(stream, -1);
+ va_list va;
+ char* buf = NULL;
+ int16_t len;
+ va_start(va, fmt);
+ len = vasprintf(&buf, fmt, va);
+ va_end(va);
+ if (len > 0) {
+ len = AP::FS().write(stream->fd, buf, len);
+ free(buf);
+ }
+
+ return len;
+}
+
+int apfs_fflush(APFS_FILE *stream)
+{
+ CHECK_STREAM(stream, EOF);
+ return 0;
+}
+
+size_t apfs_fread(void *ptr, size_t size, size_t nmemb, APFS_FILE *stream)
+{
+ CHECK_STREAM(stream, 0);
+ ssize_t ret = AP::FS().read(stream->fd, ptr, size*nmemb);
+ if (ret <= 0) {
+ stream->eof = true;
+ return 0;
+ }
+ return ret / size;
+}
+
+size_t apfs_fwrite(const void *ptr, size_t size, size_t nmemb, APFS_FILE *stream)
+{
+ CHECK_STREAM(stream, 0);
+ ssize_t ret = AP::FS().write(stream->fd, ptr, size*nmemb);
+ if (ret <= 0) {
+ stream->error = true;
+ return 0;
+ }
+ return ret / size;
+}
+
+int apfs_fputs(const char *s, APFS_FILE *stream)
+{
+ CHECK_STREAM(stream, EOF);
+ ssize_t ret = AP::FS().write(stream->fd, s, strlen(s));
+ if (ret < 0) {
+ stream->error = true;
+ return EOF;
+ }
+ return ret;
+}
+
+char *apfs_fgets(char *s, int size, APFS_FILE *stream)
+{
+ CHECK_STREAM(stream, NULL);
+ ssize_t ret = AP::FS().read(stream->fd, s, size-1);
+ if (ret < 0) {
+ stream->error = true;
+ return NULL;
+ }
+ s[ret] = 0;
+ return s;
+}
+
+void apfs_clearerr(APFS_FILE *stream)
+{
+ stream->error = false;
+}
+
+int apfs_fseek(APFS_FILE *stream, long offset, int whence)
+{
+ CHECK_STREAM(stream, EOF);
+ stream->eof = false;
+ return AP::FS().lseek(stream->fd, offset, whence);
+}
+
+int apfs_ferror(APFS_FILE *stream)
+{
+ CHECK_STREAM(stream, EOF);
+ return stream->error;
+}
+
+int apfs_fclose(APFS_FILE *stream)
+{
+ CHECK_STREAM(stream, EOF);
+ int ret = AP::FS().close(stream->fd);
+ stream->fd = -1;
+ if (stream->tmpfile_name) {
+ AP::FS().unlink(stream->tmpfile_name);
+ free(stream->tmpfile_name);
+ stream->tmpfile_name = NULL;
+ }
+ delete stream;
+ return ret;
+}
+
+APFS_FILE *apfs_tmpfile(void)
+{
+ char *fname = NULL;
+ if (asprintf(&fname, "tmp.%03u", unsigned(get_random16()) % 1000) <= 0) {
+ return NULL;
+ }
+ APFS_FILE *ret = apfs_fopen(fname, "w");
+ if (!ret) {
+ free(fname);
+ return NULL;
+ }
+ ret->tmpfile_name = fname;
+ return ret;
+}
+
+int apfs_getc(APFS_FILE *stream)
+{
+ CHECK_STREAM(stream, EOF);
+ uint8_t c;
+ if (stream->unget != -1) {
+ c = stream->unget;
+ stream->unget = -1;
+ return c;
+ }
+ ssize_t ret = AP::FS().read(stream->fd, &c, 1);
+ if (ret <= 0) {
+ stream->eof = true;
+ return EOF;
+ }
+ return c;
+}
+
+int apfs_ungetc(int c, APFS_FILE *stream)
+{
+ CHECK_STREAM(stream, EOF);
+ stream->unget = c;
+ stream->eof = false;
+ return c;
+}
+
+int apfs_feof(APFS_FILE *stream)
+{
+ return stream->eof;
+}
+
+APFS_FILE *apfs_freopen(const char *pathname, const char *mode, APFS_FILE *stream)
+{
+ CHECK_STREAM(stream, NULL);
+ int ret = AP::FS().close(stream->fd);
+ if (ret < 0) {
+ return NULL;
+ }
+ if (stream->tmpfile_name) {
+ AP::FS().unlink(stream->tmpfile_name);
+ free(stream->tmpfile_name);
+ stream->tmpfile_name = NULL;
+ }
+ stream->fd = AP::FS().open(pathname, posix_fopen_modes_to_open(mode));
+ stream->error = false;
+ stream->eof = false;
+ stream->unget = -1;
+ return stream;
+}
+
+long apfs_ftell(APFS_FILE *stream)
+{
+ CHECK_STREAM(stream, EOF);
+ return AP::FS().lseek(stream->fd, 0, SEEK_CUR);
+}
+
+int apfs_remove(const char *pathname)
+{
+ return AP::FS().unlink(pathname);
+}
+
+#endif // HAVE_FILESYSTEM_SUPPORT
diff --git a/libraries/AP_Filesystem/posix_compat.h b/libraries/AP_Filesystem/posix_compat.h
new file mode 100644
index 0000000000..ecbe3c6a0d
--- /dev/null
+++ b/libraries/AP_Filesystem/posix_compat.h
@@ -0,0 +1,94 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+/*
+ compatibility with posix APIs using AP_Filesystem
+ */
+#pragma once
+
+#include
+#include
+#include
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*
+ these are here to allow lua to build on HAL_ChibiOS
+ */
+
+typedef struct apfs_file APFS_FILE;
+
+APFS_FILE *apfs_fopen(const char *pathname, const char *mode);
+int apfs_fprintf(APFS_FILE *stream, const char *format, ...);
+int apfs_fflush(APFS_FILE *stream);
+size_t apfs_fread(void *ptr, size_t size, size_t nmemb, APFS_FILE *stream);
+size_t apfs_fwrite(const void *ptr, size_t size, size_t nmemb, APFS_FILE *stream);
+int apfs_fputs(const char *s, APFS_FILE *stream);
+char *apfs_fgets(char *s, int size, APFS_FILE *stream);
+void apfs_clearerr(APFS_FILE *stream);
+int apfs_fseek(APFS_FILE *stream, long offset, int whence);
+int apfs_ferror(APFS_FILE *stream);
+int apfs_fclose(APFS_FILE *stream);
+APFS_FILE *apfs_tmpfile(void);
+int apfs_getc(APFS_FILE *stream);
+int apfs_ungetc(int c, APFS_FILE *stream);
+int apfs_feof(APFS_FILE *stream);
+long apfs_ftell(APFS_FILE *stream);
+APFS_FILE *apfs_freopen(const char *pathname, const char *mode, APFS_FILE *stream);
+int apfs_remove(const char *pathname);
+
+#undef stdin
+#undef stdout
+#undef stderr
+#define stdin ((APFS_FILE*)1)
+#define stdout ((APFS_FILE*)2)
+#define stderr ((APFS_FILE*)3)
+
+#undef BUFSIZ
+#define BUFSIZ 256
+#define EOF (-1)
+
+#ifndef SEEK_SET
+#define SEEK_SET 0
+#define SEEK_CUR 1
+#define SEEK_END 2
+#endif
+
+#define FILE APFS_FILE
+#define fopen(p,m) apfs_fopen(p,m)
+#define fprintf(stream, format, args...) apfs_fprintf(stream, format, ##args)
+#define fflush(s) apfs_fflush(s)
+#define fread(ptr,size,nmemb, stream) apfs_fread(ptr, size, nmemb, stream)
+#define fwrite(ptr, size, nmemb, stream) apfs_fwrite(ptr, size, nmemb, stream)
+#define fputs(s, stream) apfs_fputs(s, stream)
+#define fgets(s, size, stream) apfs_fgets(s, size, stream)
+#define clearerr(stream) apfs_clearerr(stream)
+#define fseek(stream, offset, whence) apfs_fseek(stream, offset, whence)
+#define ferror(stream) apfs_ferror(stream)
+#define fclose(stream) apfs_fclose(stream)
+#define tmpfile() apfs_tmpfile()
+#undef getc
+#define getc(stream) apfs_getc(stream)
+#define ungetc(c, stream) apfs_ungetc(c, stream)
+#define feof(stream) apfs_ferror(stream)
+#define ftell(stream) apfs_ftell(stream)
+#define freopen(pathname, mode, stream) apfs_freopen(pathname, mode, stream)
+#define remove(pathname) apfs_remove(pathname)
+int sprintf(char *str, const char *format, ...);
+
+#ifdef __cplusplus
+}
+#endif