mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-30 12:38:33 -04:00
AP_Filesystem: added VFS system for multiple backends
This commit is contained in:
parent
4ca11224ba
commit
6583f7c13e
@ -19,6 +19,190 @@
|
|||||||
|
|
||||||
static AP_Filesystem fs;
|
static AP_Filesystem fs;
|
||||||
|
|
||||||
|
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
|
||||||
|
#include "AP_Filesystem_FATFS.h"
|
||||||
|
static AP_Filesystem_FATFS fs_local;
|
||||||
|
#endif
|
||||||
|
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX || CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||||
|
#include "AP_Filesystem_posix.h"
|
||||||
|
static AP_Filesystem_Posix fs_local;
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef HAL_HAVE_AP_ROMFS_EMBEDDED_H
|
||||||
|
#include "AP_Filesystem_ROMFS.h"
|
||||||
|
static AP_Filesystem_ROMFS fs_romfs;
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/*
|
||||||
|
mapping from filesystem prefix to backend
|
||||||
|
*/
|
||||||
|
const AP_Filesystem::Backend AP_Filesystem::backends[] = {
|
||||||
|
{ nullptr, fs_local },
|
||||||
|
#ifdef HAL_HAVE_AP_ROMFS_EMBEDDED_H
|
||||||
|
{ ":ROMFS/", fs_romfs },
|
||||||
|
#endif
|
||||||
|
};
|
||||||
|
|
||||||
|
#define MAX_FD_PER_BACKEND 256U
|
||||||
|
#define NUM_BACKENDS ARRAY_SIZE(backends)
|
||||||
|
#define LOCAL_BACKEND backends[0];
|
||||||
|
#define BACKEND_IDX(backend) (&(backend) - &backends[0])
|
||||||
|
|
||||||
|
/*
|
||||||
|
find backend by path
|
||||||
|
*/
|
||||||
|
const AP_Filesystem::Backend &AP_Filesystem::backend_by_path(const char *&path) const
|
||||||
|
{
|
||||||
|
for (uint8_t i=1; i<NUM_BACKENDS; i++) {
|
||||||
|
const uint8_t plen = strlen(backends[i].prefix);
|
||||||
|
if (strncmp(path, backends[i].prefix, plen) == 0) {
|
||||||
|
path += plen;
|
||||||
|
return backends[i];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
// default to local filesystem
|
||||||
|
return LOCAL_BACKEND;
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
return backend by file descriptor
|
||||||
|
*/
|
||||||
|
const AP_Filesystem::Backend &AP_Filesystem::backend_by_fd(int &fd) const
|
||||||
|
{
|
||||||
|
if (fd < 0 || uint32_t(fd) >= NUM_BACKENDS*MAX_FD_PER_BACKEND) {
|
||||||
|
return LOCAL_BACKEND;
|
||||||
|
}
|
||||||
|
const uint8_t idx = uint32_t(fd) / MAX_FD_PER_BACKEND;
|
||||||
|
fd -= idx * MAX_FD_PER_BACKEND;
|
||||||
|
return backends[idx];
|
||||||
|
}
|
||||||
|
|
||||||
|
int AP_Filesystem::open(const char *fname, int flags)
|
||||||
|
{
|
||||||
|
const Backend &backend = backend_by_path(fname);
|
||||||
|
int fd = backend.fs.open(fname, flags);
|
||||||
|
if (fd < 0) {
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
if (uint32_t(fd) >= MAX_FD_PER_BACKEND) {
|
||||||
|
backend.fs.close(fd);
|
||||||
|
errno = ERANGE;
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
// offset fd so we can recognise the backend
|
||||||
|
const uint8_t idx = (&backend - &backends[0]);
|
||||||
|
fd += idx * MAX_FD_PER_BACKEND;
|
||||||
|
return fd;
|
||||||
|
}
|
||||||
|
|
||||||
|
int AP_Filesystem::close(int fd)
|
||||||
|
{
|
||||||
|
const Backend &backend = backend_by_fd(fd);
|
||||||
|
return backend.fs.close(fd);
|
||||||
|
}
|
||||||
|
|
||||||
|
ssize_t AP_Filesystem::read(int fd, void *buf, size_t count)
|
||||||
|
{
|
||||||
|
const Backend &backend = backend_by_fd(fd);
|
||||||
|
return backend.fs.read(fd, buf, count);
|
||||||
|
}
|
||||||
|
|
||||||
|
ssize_t AP_Filesystem::write(int fd, const void *buf, size_t count)
|
||||||
|
{
|
||||||
|
const Backend &backend = backend_by_fd(fd);
|
||||||
|
return backend.fs.write(fd, buf, count);
|
||||||
|
}
|
||||||
|
|
||||||
|
int AP_Filesystem::fsync(int fd)
|
||||||
|
{
|
||||||
|
const Backend &backend = backend_by_fd(fd);
|
||||||
|
return backend.fs.fsync(fd);
|
||||||
|
}
|
||||||
|
|
||||||
|
off_t AP_Filesystem::lseek(int fd, off_t offset, int seek_from)
|
||||||
|
{
|
||||||
|
const Backend &backend = backend_by_fd(fd);
|
||||||
|
return backend.fs.lseek(fd, offset, seek_from);
|
||||||
|
}
|
||||||
|
|
||||||
|
int AP_Filesystem::stat(const char *pathname, struct stat *stbuf)
|
||||||
|
{
|
||||||
|
const Backend &backend = backend_by_path(pathname);
|
||||||
|
return backend.fs.stat(pathname, stbuf);
|
||||||
|
}
|
||||||
|
|
||||||
|
int AP_Filesystem::unlink(const char *pathname)
|
||||||
|
{
|
||||||
|
const Backend &backend = backend_by_path(pathname);
|
||||||
|
return backend.fs.unlink(pathname);
|
||||||
|
}
|
||||||
|
|
||||||
|
int AP_Filesystem::mkdir(const char *pathname)
|
||||||
|
{
|
||||||
|
const Backend &backend = backend_by_path(pathname);
|
||||||
|
return backend.fs.mkdir(pathname);
|
||||||
|
}
|
||||||
|
|
||||||
|
AP_Filesystem::DirHandle *AP_Filesystem::opendir(const char *pathname)
|
||||||
|
{
|
||||||
|
const Backend &backend = backend_by_path(pathname);
|
||||||
|
DirHandle *h = new DirHandle;
|
||||||
|
if (!h) {
|
||||||
|
return nullptr;
|
||||||
|
}
|
||||||
|
h->dir = backend.fs.opendir(pathname);
|
||||||
|
if (h->dir == nullptr) {
|
||||||
|
delete h;
|
||||||
|
return nullptr;
|
||||||
|
}
|
||||||
|
h->fs_index = BACKEND_IDX(backend);
|
||||||
|
return h;
|
||||||
|
}
|
||||||
|
|
||||||
|
struct dirent *AP_Filesystem::readdir(DirHandle *dirp)
|
||||||
|
{
|
||||||
|
if (!dirp) {
|
||||||
|
return nullptr;
|
||||||
|
}
|
||||||
|
const Backend &backend = backends[dirp->fs_index];
|
||||||
|
return backend.fs.readdir(dirp->dir);
|
||||||
|
}
|
||||||
|
|
||||||
|
int AP_Filesystem::closedir(DirHandle *dirp)
|
||||||
|
{
|
||||||
|
if (!dirp) {
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
const Backend &backend = backends[dirp->fs_index];
|
||||||
|
int ret = backend.fs.closedir(dirp->dir);
|
||||||
|
delete dirp;
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
// return free disk space in bytes
|
||||||
|
int64_t AP_Filesystem::disk_free(const char *path)
|
||||||
|
{
|
||||||
|
const Backend &backend = backend_by_path(path);
|
||||||
|
return backend.fs.disk_free(path);
|
||||||
|
}
|
||||||
|
|
||||||
|
// return total disk space in bytes
|
||||||
|
int64_t AP_Filesystem::disk_space(const char *path)
|
||||||
|
{
|
||||||
|
const Backend &backend = backend_by_path(path);
|
||||||
|
return backend.fs.disk_space(path);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
set mtime on a file
|
||||||
|
*/
|
||||||
|
bool AP_Filesystem::set_mtime(const char *filename, const time_t mtime_sec)
|
||||||
|
{
|
||||||
|
const Backend &backend = backend_by_path(filename);
|
||||||
|
return backend.fs.set_mtime(filename, mtime_sec);
|
||||||
|
}
|
||||||
|
|
||||||
namespace AP
|
namespace AP
|
||||||
{
|
{
|
||||||
AP_Filesystem &FS()
|
AP_Filesystem &FS()
|
||||||
|
@ -33,6 +33,11 @@
|
|||||||
#endif
|
#endif
|
||||||
|
|
||||||
class AP_Filesystem {
|
class AP_Filesystem {
|
||||||
|
private:
|
||||||
|
struct DirHandle {
|
||||||
|
uint8_t fs_index;
|
||||||
|
void *dir;
|
||||||
|
};
|
||||||
|
|
||||||
public:
|
public:
|
||||||
AP_Filesystem() {}
|
AP_Filesystem() {}
|
||||||
@ -47,9 +52,10 @@ public:
|
|||||||
int stat(const char *pathname, struct stat *stbuf);
|
int stat(const char *pathname, struct stat *stbuf);
|
||||||
int unlink(const char *pathname);
|
int unlink(const char *pathname);
|
||||||
int mkdir(const char *pathname);
|
int mkdir(const char *pathname);
|
||||||
DIR *opendir(const char *pathname);
|
|
||||||
struct dirent *readdir(DIR *dirp);
|
DirHandle *opendir(const char *pathname);
|
||||||
int closedir(DIR *dirp);
|
struct dirent *readdir(DirHandle *dirp);
|
||||||
|
int closedir(DirHandle *dirp);
|
||||||
|
|
||||||
// return free disk space in bytes, -1 on error
|
// return free disk space in bytes, -1 on error
|
||||||
int64_t disk_free(const char *path);
|
int64_t disk_free(const char *path);
|
||||||
@ -59,6 +65,23 @@ public:
|
|||||||
|
|
||||||
// set modification time on a file
|
// set modification time on a file
|
||||||
bool set_mtime(const char *filename, const time_t mtime_sec);
|
bool set_mtime(const char *filename, const time_t mtime_sec);
|
||||||
|
|
||||||
|
private:
|
||||||
|
struct Backend {
|
||||||
|
const char *prefix;
|
||||||
|
AP_Filesystem_Backend &fs;
|
||||||
|
};
|
||||||
|
static const struct Backend backends[];
|
||||||
|
|
||||||
|
/*
|
||||||
|
find backend by path
|
||||||
|
*/
|
||||||
|
const Backend &backend_by_path(const char *&path) const;
|
||||||
|
|
||||||
|
/*
|
||||||
|
find backend by open fd
|
||||||
|
*/
|
||||||
|
const Backend &backend_by_fd(int &fd) const;
|
||||||
};
|
};
|
||||||
|
|
||||||
namespace AP {
|
namespace AP {
|
||||||
|
@ -276,7 +276,7 @@ static bool remount_file_system(void)
|
|||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
int AP_Filesystem::open(const char *pathname, int flags)
|
int AP_Filesystem_FATFS::open(const char *pathname, int flags)
|
||||||
{
|
{
|
||||||
int fileno;
|
int fileno;
|
||||||
int fatfs_modes;
|
int fatfs_modes;
|
||||||
@ -352,7 +352,7 @@ int AP_Filesystem::open(const char *pathname, int flags)
|
|||||||
return fileno;
|
return fileno;
|
||||||
}
|
}
|
||||||
|
|
||||||
int AP_Filesystem::close(int fileno)
|
int AP_Filesystem_FATFS::close(int fileno)
|
||||||
{
|
{
|
||||||
FAT_FILE *stream;
|
FAT_FILE *stream;
|
||||||
FIL *fh;
|
FIL *fh;
|
||||||
@ -382,7 +382,7 @@ int AP_Filesystem::close(int fileno)
|
|||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
ssize_t AP_Filesystem::read(int fd, void *buf, size_t count)
|
ssize_t AP_Filesystem_FATFS::read(int fd, void *buf, size_t count)
|
||||||
{
|
{
|
||||||
UINT bytes = count;
|
UINT bytes = count;
|
||||||
int res;
|
int res;
|
||||||
@ -431,7 +431,7 @@ ssize_t AP_Filesystem::read(int fd, void *buf, size_t count)
|
|||||||
return (ssize_t)total;
|
return (ssize_t)total;
|
||||||
}
|
}
|
||||||
|
|
||||||
ssize_t AP_Filesystem::write(int fd, const void *buf, size_t count)
|
ssize_t AP_Filesystem_FATFS::write(int fd, const void *buf, size_t count)
|
||||||
{
|
{
|
||||||
UINT bytes = count;
|
UINT bytes = count;
|
||||||
FRESULT res;
|
FRESULT res;
|
||||||
@ -479,7 +479,7 @@ ssize_t AP_Filesystem::write(int fd, const void *buf, size_t count)
|
|||||||
return (ssize_t)total;
|
return (ssize_t)total;
|
||||||
}
|
}
|
||||||
|
|
||||||
int AP_Filesystem::fsync(int fileno)
|
int AP_Filesystem_FATFS::fsync(int fileno)
|
||||||
{
|
{
|
||||||
FAT_FILE *stream;
|
FAT_FILE *stream;
|
||||||
FIL *fh;
|
FIL *fh;
|
||||||
@ -508,7 +508,7 @@ int AP_Filesystem::fsync(int fileno)
|
|||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
off_t AP_Filesystem::lseek(int fileno, off_t position, int whence)
|
off_t AP_Filesystem_FATFS::lseek(int fileno, off_t position, int whence)
|
||||||
{
|
{
|
||||||
FRESULT res;
|
FRESULT res;
|
||||||
FIL *fh;
|
FIL *fh;
|
||||||
@ -599,7 +599,7 @@ static time_t fat_time_to_unix(uint16_t date, uint16_t time)
|
|||||||
return unix;
|
return unix;
|
||||||
}
|
}
|
||||||
|
|
||||||
int AP_Filesystem::stat(const char *name, struct stat *buf)
|
int AP_Filesystem_FATFS::stat(const char *name, struct stat *buf)
|
||||||
{
|
{
|
||||||
FILINFO info;
|
FILINFO info;
|
||||||
int res;
|
int res;
|
||||||
@ -667,7 +667,7 @@ int AP_Filesystem::stat(const char *name, struct stat *buf)
|
|||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
int AP_Filesystem::unlink(const char *pathname)
|
int AP_Filesystem_FATFS::unlink(const char *pathname)
|
||||||
{
|
{
|
||||||
WITH_SEMAPHORE(sem);
|
WITH_SEMAPHORE(sem);
|
||||||
|
|
||||||
@ -680,7 +680,7 @@ int AP_Filesystem::unlink(const char *pathname)
|
|||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
int AP_Filesystem::mkdir(const char *pathname)
|
int AP_Filesystem_FATFS::mkdir(const char *pathname)
|
||||||
{
|
{
|
||||||
WITH_SEMAPHORE(sem);
|
WITH_SEMAPHORE(sem);
|
||||||
|
|
||||||
@ -703,7 +703,7 @@ struct DIR_Wrapper {
|
|||||||
struct dirent de;
|
struct dirent de;
|
||||||
};
|
};
|
||||||
|
|
||||||
DIR *AP_Filesystem::opendir(const char *pathdir)
|
void *AP_Filesystem_FATFS::opendir(const char *pathdir)
|
||||||
{
|
{
|
||||||
WITH_SEMAPHORE(sem);
|
WITH_SEMAPHORE(sem);
|
||||||
|
|
||||||
@ -730,9 +730,10 @@ DIR *AP_Filesystem::opendir(const char *pathdir)
|
|||||||
return &ret->d;
|
return &ret->d;
|
||||||
}
|
}
|
||||||
|
|
||||||
struct dirent *AP_Filesystem::readdir(DIR *dirp)
|
struct dirent *AP_Filesystem_FATFS::readdir(void *dirp_void)
|
||||||
{
|
{
|
||||||
WITH_SEMAPHORE(sem);
|
WITH_SEMAPHORE(sem);
|
||||||
|
DIR *dirp = (DIR *)dirp_void;
|
||||||
|
|
||||||
struct DIR_Wrapper *d = (struct DIR_Wrapper *)dirp;
|
struct DIR_Wrapper *d = (struct DIR_Wrapper *)dirp;
|
||||||
if (!d) {
|
if (!d) {
|
||||||
@ -760,8 +761,9 @@ struct dirent *AP_Filesystem::readdir(DIR *dirp)
|
|||||||
return &d->de;
|
return &d->de;
|
||||||
}
|
}
|
||||||
|
|
||||||
int AP_Filesystem::closedir(DIR *dirp)
|
int AP_Filesystem_FATFS::closedir(void *dirp_void)
|
||||||
{
|
{
|
||||||
|
DIR *dirp = (DIR *)dirp_void;
|
||||||
WITH_SEMAPHORE(sem);
|
WITH_SEMAPHORE(sem);
|
||||||
|
|
||||||
struct DIR_Wrapper *d = (struct DIR_Wrapper *)dirp;
|
struct DIR_Wrapper *d = (struct DIR_Wrapper *)dirp;
|
||||||
@ -780,7 +782,7 @@ int AP_Filesystem::closedir(DIR *dirp)
|
|||||||
}
|
}
|
||||||
|
|
||||||
// return free disk space in bytes
|
// return free disk space in bytes
|
||||||
int64_t AP_Filesystem::disk_free(const char *path)
|
int64_t AP_Filesystem_FATFS::disk_free(const char *path)
|
||||||
{
|
{
|
||||||
WITH_SEMAPHORE(sem);
|
WITH_SEMAPHORE(sem);
|
||||||
|
|
||||||
@ -801,7 +803,7 @@ int64_t AP_Filesystem::disk_free(const char *path)
|
|||||||
}
|
}
|
||||||
|
|
||||||
// return total disk space in bytes
|
// return total disk space in bytes
|
||||||
int64_t AP_Filesystem::disk_space(const char *path)
|
int64_t AP_Filesystem_FATFS::disk_space(const char *path)
|
||||||
{
|
{
|
||||||
WITH_SEMAPHORE(sem);
|
WITH_SEMAPHORE(sem);
|
||||||
|
|
||||||
@ -841,7 +843,7 @@ static void unix_time_to_fat(time_t epoch, uint16_t &date, uint16_t &time)
|
|||||||
/*
|
/*
|
||||||
set mtime on a file
|
set mtime on a file
|
||||||
*/
|
*/
|
||||||
bool AP_Filesystem::set_mtime(const char *filename, const time_t mtime_sec)
|
bool AP_Filesystem_FATFS::set_mtime(const char *filename, const time_t mtime_sec)
|
||||||
{
|
{
|
||||||
FILINFO fno;
|
FILINFO fno;
|
||||||
uint16_t fdate, ftime;
|
uint16_t fdate, ftime;
|
||||||
|
@ -2,12 +2,15 @@
|
|||||||
FATFS backend for AP_Filesystem
|
FATFS backend for AP_Filesystem
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
#include <sys/types.h>
|
#include <sys/types.h>
|
||||||
#include <sys/stat.h>
|
#include <sys/stat.h>
|
||||||
#include <fcntl.h>
|
#include <fcntl.h>
|
||||||
#include <errno.h>
|
#include <errno.h>
|
||||||
#include <stddef.h>
|
#include <stddef.h>
|
||||||
#include <ff.h>
|
#include <ff.h>
|
||||||
|
#include "AP_Filesystem_backend.h"
|
||||||
|
|
||||||
// Seek offset macros
|
// Seek offset macros
|
||||||
#define SEEK_SET 0
|
#define SEEK_SET 0
|
||||||
@ -27,3 +30,30 @@ struct dirent {
|
|||||||
char d_name[MAX_NAME_LEN]; /* filename */
|
char d_name[MAX_NAME_LEN]; /* filename */
|
||||||
uint8_t d_type;
|
uint8_t d_type;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
class AP_Filesystem_FATFS : public AP_Filesystem_Backend
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
// functions that closely match the equivalent posix calls
|
||||||
|
int open(const char *fname, int flags) override;
|
||||||
|
int close(int fd) override;
|
||||||
|
ssize_t read(int fd, void *buf, size_t count) override;
|
||||||
|
ssize_t write(int fd, const void *buf, size_t count) override;
|
||||||
|
int fsync(int fd) override;
|
||||||
|
off_t lseek(int fd, off_t offset, int whence) override;
|
||||||
|
int stat(const char *pathname, struct stat *stbuf) override;
|
||||||
|
int unlink(const char *pathname) override;
|
||||||
|
int mkdir(const char *pathname) override;
|
||||||
|
void *opendir(const char *pathname) override;
|
||||||
|
struct dirent *readdir(void *dirp) override;
|
||||||
|
int closedir(void *dirp) override;
|
||||||
|
|
||||||
|
// return free disk space in bytes, -1 on error
|
||||||
|
int64_t disk_free(const char *path) override;
|
||||||
|
|
||||||
|
// return total disk space in bytes, -1 on error
|
||||||
|
int64_t disk_space(const char *path) override;
|
||||||
|
|
||||||
|
// set modification time on a file
|
||||||
|
bool set_mtime(const char *filename, const time_t mtime_sec) override;
|
||||||
|
};
|
||||||
|
207
libraries/AP_Filesystem/AP_Filesystem_ROMFS.cpp
Normal file
207
libraries/AP_Filesystem/AP_Filesystem_ROMFS.cpp
Normal file
@ -0,0 +1,207 @@
|
|||||||
|
/*
|
||||||
|
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 <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
/*
|
||||||
|
ArduPilot filesystem interface for ROMFS
|
||||||
|
*/
|
||||||
|
#include "AP_Filesystem.h"
|
||||||
|
#include "AP_Filesystem_ROMFS.h"
|
||||||
|
#include <AP_HAL/AP_HAL.h>
|
||||||
|
#include <AP_Math/AP_Math.h>
|
||||||
|
#include <AP_ROMFS/AP_ROMFS.h>
|
||||||
|
|
||||||
|
#if defined(HAL_HAVE_AP_ROMFS_EMBEDDED_H) && HAVE_FILESYSTEM_SUPPORT
|
||||||
|
|
||||||
|
int AP_Filesystem_ROMFS::open(const char *fname, int flags)
|
||||||
|
{
|
||||||
|
uint8_t idx;
|
||||||
|
for (idx=0; idx<max_open_file; idx++) {
|
||||||
|
if (file[idx].data == nullptr) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (idx == max_open_file) {
|
||||||
|
errno = ENFILE;
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
if (file[idx].data != nullptr) {
|
||||||
|
errno = EBUSY;
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
file[idx].data = AP_ROMFS::find_decompress(fname, file[idx].size);
|
||||||
|
if (file[idx].data == nullptr) {
|
||||||
|
errno = ENOENT;
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
file[idx].ofs = 0;
|
||||||
|
return idx;
|
||||||
|
}
|
||||||
|
|
||||||
|
int AP_Filesystem_ROMFS::close(int fd)
|
||||||
|
{
|
||||||
|
if (fd < 0 || fd >= max_open_file || file[fd].data == nullptr) {
|
||||||
|
errno = EBADF;
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
AP_ROMFS::free(file[fd].data);
|
||||||
|
file[fd].data = nullptr;
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
ssize_t AP_Filesystem_ROMFS::read(int fd, void *buf, size_t count)
|
||||||
|
{
|
||||||
|
if (fd < 0 || fd >= max_open_file || file[fd].data == nullptr) {
|
||||||
|
errno = EBADF;
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
count = MIN(file[fd].size - file[fd].ofs, count);
|
||||||
|
if (count == 0) {
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
memcpy(buf, &file[fd].data[file[fd].ofs], count);
|
||||||
|
file[fd].ofs += count;
|
||||||
|
return count;
|
||||||
|
}
|
||||||
|
|
||||||
|
ssize_t AP_Filesystem_ROMFS::write(int fd, const void *buf, size_t count)
|
||||||
|
{
|
||||||
|
errno = EROFS;
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
int AP_Filesystem_ROMFS::fsync(int fd)
|
||||||
|
{
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
off_t AP_Filesystem_ROMFS::lseek(int fd, off_t offset, int seek_from)
|
||||||
|
{
|
||||||
|
if (fd < 0 || fd >= max_open_file || file[fd].data == nullptr) {
|
||||||
|
errno = EBADF;
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
switch (seek_from) {
|
||||||
|
case SEEK_SET:
|
||||||
|
file[fd].ofs = MIN(file[fd].size, offset);
|
||||||
|
break;
|
||||||
|
case SEEK_CUR:
|
||||||
|
file[fd].ofs = MIN(file[fd].size, offset+file[fd].ofs);
|
||||||
|
break;
|
||||||
|
case SEEK_END:
|
||||||
|
file[fd].ofs = file[fd].size;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
return file[fd].ofs;
|
||||||
|
}
|
||||||
|
|
||||||
|
int AP_Filesystem_ROMFS::stat(const char *name, struct stat *stbuf)
|
||||||
|
{
|
||||||
|
uint32_t size;
|
||||||
|
const uint8_t *data = AP_ROMFS::find_decompress(name, size);
|
||||||
|
if (data == nullptr) {
|
||||||
|
errno = ENOENT;
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
AP_ROMFS::free(data);
|
||||||
|
memset(stbuf, 0, sizeof(*stbuf));
|
||||||
|
stbuf->st_size = size;
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
int AP_Filesystem_ROMFS::unlink(const char *pathname)
|
||||||
|
{
|
||||||
|
errno = EROFS;
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
int AP_Filesystem_ROMFS::mkdir(const char *pathname)
|
||||||
|
{
|
||||||
|
errno = EROFS;
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
void *AP_Filesystem_ROMFS::opendir(const char *pathname)
|
||||||
|
{
|
||||||
|
uint8_t idx;
|
||||||
|
for (idx=0; idx<max_open_dir; idx++) {
|
||||||
|
if (dir[idx].path == nullptr) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (idx == max_open_dir) {
|
||||||
|
errno = ENFILE;
|
||||||
|
return nullptr;
|
||||||
|
}
|
||||||
|
dir[idx].ofs = 0;
|
||||||
|
dir[idx].path = strdup(pathname);
|
||||||
|
if (!dir[idx].path) {
|
||||||
|
return nullptr;
|
||||||
|
}
|
||||||
|
return (void*)&dir[idx];
|
||||||
|
}
|
||||||
|
|
||||||
|
struct dirent *AP_Filesystem_ROMFS::readdir(void *dirp)
|
||||||
|
{
|
||||||
|
uint32_t idx = ((rdir*)dirp) - &dir[0];
|
||||||
|
if (idx >= max_open_dir) {
|
||||||
|
errno = EBADF;
|
||||||
|
return nullptr;
|
||||||
|
}
|
||||||
|
const char *name = AP_ROMFS::dir_list(dir[idx].path, dir[idx].ofs);
|
||||||
|
if (!name) {
|
||||||
|
return nullptr;
|
||||||
|
}
|
||||||
|
const uint32_t plen = strlen(dir[idx].path);
|
||||||
|
if (strncmp(name, dir[idx].path, plen) != 0 || name[plen] != '/') {
|
||||||
|
return nullptr;
|
||||||
|
}
|
||||||
|
name += plen + 1;
|
||||||
|
dir[idx].de.d_type = DT_REG;
|
||||||
|
strncpy(dir[idx].de.d_name, name, sizeof(dir[idx].de.d_name));
|
||||||
|
return &dir[idx].de;
|
||||||
|
}
|
||||||
|
|
||||||
|
int AP_Filesystem_ROMFS::closedir(void *dirp)
|
||||||
|
{
|
||||||
|
uint32_t idx = ((rdir *)dirp) - &dir[0];
|
||||||
|
if (idx >= max_open_dir) {
|
||||||
|
errno = EBADF;
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
free(dir[idx].path);
|
||||||
|
dir[idx].path = nullptr;
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
// return free disk space in bytes
|
||||||
|
int64_t AP_Filesystem_ROMFS::disk_free(const char *path)
|
||||||
|
{
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
// return total disk space in bytes
|
||||||
|
int64_t AP_Filesystem_ROMFS::disk_space(const char *path)
|
||||||
|
{
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
set mtime on a file
|
||||||
|
*/
|
||||||
|
bool AP_Filesystem_ROMFS::set_mtime(const char *filename, const time_t mtime_sec)
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif // HAL_HAVE_AP_ROMFS_EMBEDDED_H
|
66
libraries/AP_Filesystem/AP_Filesystem_ROMFS.h
Normal file
66
libraries/AP_Filesystem/AP_Filesystem_ROMFS.h
Normal file
@ -0,0 +1,66 @@
|
|||||||
|
/*
|
||||||
|
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 <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include "AP_Filesystem_backend.h"
|
||||||
|
|
||||||
|
#if HAVE_FILESYSTEM_SUPPORT
|
||||||
|
|
||||||
|
class AP_Filesystem_ROMFS : public AP_Filesystem_Backend
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
// functions that closely match the equivalent posix calls
|
||||||
|
int open(const char *fname, int flags) override;
|
||||||
|
int close(int fd) override;
|
||||||
|
ssize_t read(int fd, void *buf, size_t count) override;
|
||||||
|
ssize_t write(int fd, const void *buf, size_t count) override;
|
||||||
|
int fsync(int fd) override;
|
||||||
|
off_t lseek(int fd, off_t offset, int whence) override;
|
||||||
|
int stat(const char *pathname, struct stat *stbuf) override;
|
||||||
|
int unlink(const char *pathname) override;
|
||||||
|
int mkdir(const char *pathname) override;
|
||||||
|
void *opendir(const char *pathname) override;
|
||||||
|
struct dirent *readdir(void *dirp) override;
|
||||||
|
int closedir(void *dirp) override;
|
||||||
|
|
||||||
|
// return free disk space in bytes, -1 on error
|
||||||
|
int64_t disk_free(const char *path) override;
|
||||||
|
|
||||||
|
// return total disk space in bytes, -1 on error
|
||||||
|
int64_t disk_space(const char *path) override;
|
||||||
|
|
||||||
|
// set modification time on a file
|
||||||
|
bool set_mtime(const char *filename, const time_t mtime_sec) override;
|
||||||
|
|
||||||
|
private:
|
||||||
|
// only allow up to 4 files at a time
|
||||||
|
static constexpr uint8_t max_open_file = 4;
|
||||||
|
static constexpr uint8_t max_open_dir = 4;
|
||||||
|
struct rfile {
|
||||||
|
const uint8_t *data;
|
||||||
|
uint32_t size;
|
||||||
|
uint32_t ofs;
|
||||||
|
} file[max_open_file];
|
||||||
|
|
||||||
|
// allow up to 4 directory opens
|
||||||
|
struct rdir {
|
||||||
|
char *path;
|
||||||
|
uint16_t ofs;
|
||||||
|
struct dirent de;
|
||||||
|
} dir[max_open_dir];
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // HAVE_FILESYSTEM_SUPPORT
|
53
libraries/AP_Filesystem/AP_Filesystem_backend.h
Normal file
53
libraries/AP_Filesystem/AP_Filesystem_backend.h
Normal file
@ -0,0 +1,53 @@
|
|||||||
|
/*
|
||||||
|
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 <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
/*
|
||||||
|
ArduPilot filesystem backend interface.
|
||||||
|
*/
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
#include <AP_HAL/AP_HAL_Boards.h>
|
||||||
|
|
||||||
|
#include "AP_Filesystem_Available.h"
|
||||||
|
|
||||||
|
#if HAVE_FILESYSTEM_SUPPORT
|
||||||
|
class AP_Filesystem_Backend {
|
||||||
|
|
||||||
|
public:
|
||||||
|
// functions that closely match the equivalent posix calls
|
||||||
|
virtual int open(const char *fname, int flags) = 0;
|
||||||
|
virtual int close(int fd) = 0;
|
||||||
|
virtual ssize_t read(int fd, void *buf, size_t count) = 0;
|
||||||
|
virtual ssize_t write(int fd, const void *buf, size_t count) = 0;
|
||||||
|
virtual int fsync(int fd) = 0;
|
||||||
|
virtual off_t lseek(int fd, off_t offset, int whence) = 0;
|
||||||
|
virtual int stat(const char *pathname, struct stat *stbuf) = 0;
|
||||||
|
virtual int unlink(const char *pathname) = 0;
|
||||||
|
virtual int mkdir(const char *pathname) = 0;
|
||||||
|
virtual void *opendir(const char *pathname) = 0;
|
||||||
|
virtual struct dirent *readdir(void *dirp) = 0;
|
||||||
|
virtual int closedir(void *dirp) = 0;
|
||||||
|
|
||||||
|
// return free disk space in bytes, -1 on error
|
||||||
|
virtual int64_t disk_free(const char *path) = 0;
|
||||||
|
|
||||||
|
// return total disk space in bytes, -1 on error
|
||||||
|
virtual int64_t disk_space(const char *path) = 0;
|
||||||
|
|
||||||
|
// set modification time on a file
|
||||||
|
virtual bool set_mtime(const char *filename, const time_t mtime_sec) = 0;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif // HAVE_FILESYSTEM_SUPPORT
|
@ -48,45 +48,45 @@ static const char *map_filename(const char *fname)
|
|||||||
return fname;
|
return fname;
|
||||||
}
|
}
|
||||||
|
|
||||||
int AP_Filesystem::open(const char *fname, int flags)
|
int AP_Filesystem_Posix::open(const char *fname, int flags)
|
||||||
{
|
{
|
||||||
fname = map_filename(fname);
|
fname = map_filename(fname);
|
||||||
// we automatically add O_CLOEXEC as we always want it for ArduPilot FS usage
|
// we automatically add O_CLOEXEC as we always want it for ArduPilot FS usage
|
||||||
return ::open(fname, flags | O_CLOEXEC, 0644);
|
return ::open(fname, flags | O_CLOEXEC, 0644);
|
||||||
}
|
}
|
||||||
|
|
||||||
int AP_Filesystem::close(int fd)
|
int AP_Filesystem_Posix::close(int fd)
|
||||||
{
|
{
|
||||||
return ::close(fd);
|
return ::close(fd);
|
||||||
}
|
}
|
||||||
|
|
||||||
ssize_t AP_Filesystem::read(int fd, void *buf, size_t count)
|
ssize_t AP_Filesystem_Posix::read(int fd, void *buf, size_t count)
|
||||||
{
|
{
|
||||||
return ::read(fd, buf, count);
|
return ::read(fd, buf, count);
|
||||||
}
|
}
|
||||||
|
|
||||||
ssize_t AP_Filesystem::write(int fd, const void *buf, size_t count)
|
ssize_t AP_Filesystem_Posix::write(int fd, const void *buf, size_t count)
|
||||||
{
|
{
|
||||||
return ::write(fd, buf, count);
|
return ::write(fd, buf, count);
|
||||||
}
|
}
|
||||||
|
|
||||||
int AP_Filesystem::fsync(int fd)
|
int AP_Filesystem_Posix::fsync(int fd)
|
||||||
{
|
{
|
||||||
return ::fsync(fd);
|
return ::fsync(fd);
|
||||||
}
|
}
|
||||||
|
|
||||||
off_t AP_Filesystem::lseek(int fd, off_t offset, int seek_from)
|
off_t AP_Filesystem_Posix::lseek(int fd, off_t offset, int seek_from)
|
||||||
{
|
{
|
||||||
return ::lseek(fd, offset, seek_from);
|
return ::lseek(fd, offset, seek_from);
|
||||||
}
|
}
|
||||||
|
|
||||||
int AP_Filesystem::stat(const char *pathname, struct stat *stbuf)
|
int AP_Filesystem_Posix::stat(const char *pathname, struct stat *stbuf)
|
||||||
{
|
{
|
||||||
pathname = map_filename(pathname);
|
pathname = map_filename(pathname);
|
||||||
return ::stat(pathname, stbuf);
|
return ::stat(pathname, stbuf);
|
||||||
}
|
}
|
||||||
|
|
||||||
int AP_Filesystem::unlink(const char *pathname)
|
int AP_Filesystem_Posix::unlink(const char *pathname)
|
||||||
{
|
{
|
||||||
pathname = map_filename(pathname);
|
pathname = map_filename(pathname);
|
||||||
// we match the FATFS interface and use unlink
|
// we match the FATFS interface and use unlink
|
||||||
@ -98,30 +98,30 @@ int AP_Filesystem::unlink(const char *pathname)
|
|||||||
return ret;
|
return ret;
|
||||||
}
|
}
|
||||||
|
|
||||||
int AP_Filesystem::mkdir(const char *pathname)
|
int AP_Filesystem_Posix::mkdir(const char *pathname)
|
||||||
{
|
{
|
||||||
pathname = map_filename(pathname);
|
pathname = map_filename(pathname);
|
||||||
return ::mkdir(pathname, 0775);
|
return ::mkdir(pathname, 0775);
|
||||||
}
|
}
|
||||||
|
|
||||||
DIR *AP_Filesystem::opendir(const char *pathname)
|
void *AP_Filesystem_Posix::opendir(const char *pathname)
|
||||||
{
|
{
|
||||||
pathname = map_filename(pathname);
|
pathname = map_filename(pathname);
|
||||||
return ::opendir(pathname);
|
return (void*)::opendir(pathname);
|
||||||
}
|
}
|
||||||
|
|
||||||
struct dirent *AP_Filesystem::readdir(DIR *dirp)
|
struct dirent *AP_Filesystem_Posix::readdir(void *dirp)
|
||||||
{
|
{
|
||||||
return ::readdir(dirp);
|
return ::readdir((DIR *)dirp);
|
||||||
}
|
}
|
||||||
|
|
||||||
int AP_Filesystem::closedir(DIR *dirp)
|
int AP_Filesystem_Posix::closedir(void *dirp)
|
||||||
{
|
{
|
||||||
return ::closedir(dirp);
|
return ::closedir((DIR *)dirp);
|
||||||
}
|
}
|
||||||
|
|
||||||
// return free disk space in bytes
|
// return free disk space in bytes
|
||||||
int64_t AP_Filesystem::disk_free(const char *path)
|
int64_t AP_Filesystem_Posix::disk_free(const char *path)
|
||||||
{
|
{
|
||||||
path = map_filename(path);
|
path = map_filename(path);
|
||||||
struct statfs stats;
|
struct statfs stats;
|
||||||
@ -132,7 +132,7 @@ int64_t AP_Filesystem::disk_free(const char *path)
|
|||||||
}
|
}
|
||||||
|
|
||||||
// return total disk space in bytes
|
// return total disk space in bytes
|
||||||
int64_t AP_Filesystem::disk_space(const char *path)
|
int64_t AP_Filesystem_Posix::disk_space(const char *path)
|
||||||
{
|
{
|
||||||
path = map_filename(path);
|
path = map_filename(path);
|
||||||
struct statfs stats;
|
struct statfs stats;
|
||||||
@ -146,7 +146,7 @@ int64_t AP_Filesystem::disk_space(const char *path)
|
|||||||
/*
|
/*
|
||||||
set mtime on a file
|
set mtime on a file
|
||||||
*/
|
*/
|
||||||
bool AP_Filesystem::set_mtime(const char *filename, const time_t mtime_sec)
|
bool AP_Filesystem_Posix::set_mtime(const char *filename, const time_t mtime_sec)
|
||||||
{
|
{
|
||||||
filename = map_filename(filename);
|
filename = map_filename(filename);
|
||||||
struct utimbuf times {};
|
struct utimbuf times {};
|
||||||
|
@ -23,3 +23,32 @@
|
|||||||
#include <dirent.h>
|
#include <dirent.h>
|
||||||
#include <unistd.h>
|
#include <unistd.h>
|
||||||
#include <errno.h>
|
#include <errno.h>
|
||||||
|
#include "AP_Filesystem_backend.h"
|
||||||
|
|
||||||
|
class AP_Filesystem_Posix : public AP_Filesystem_Backend
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
// functions that closely match the equivalent posix calls
|
||||||
|
int open(const char *fname, int flags) override;
|
||||||
|
int close(int fd) override;
|
||||||
|
ssize_t read(int fd, void *buf, size_t count) override;
|
||||||
|
ssize_t write(int fd, const void *buf, size_t count) override;
|
||||||
|
int fsync(int fd) override;
|
||||||
|
off_t lseek(int fd, off_t offset, int whence) override;
|
||||||
|
int stat(const char *pathname, struct stat *stbuf) override;
|
||||||
|
int unlink(const char *pathname) override;
|
||||||
|
int mkdir(const char *pathname) override;
|
||||||
|
void *opendir(const char *pathname) override;
|
||||||
|
struct dirent *readdir(void *dirp) override;
|
||||||
|
int closedir(void *dirp) override;
|
||||||
|
|
||||||
|
// return free disk space in bytes, -1 on error
|
||||||
|
int64_t disk_free(const char *path) override;
|
||||||
|
|
||||||
|
// return total disk space in bytes, -1 on error
|
||||||
|
int64_t disk_space(const char *path) override;
|
||||||
|
|
||||||
|
// set modification time on a file
|
||||||
|
bool set_mtime(const char *filename, const time_t mtime_sec) override;
|
||||||
|
};
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user