diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index 607bff908edb0..688e61142c6f5 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -394,13 +394,22 @@ def configure_env(self, cfg, env): if not cfg.check_SFML(env): cfg.fatal("Failed to find SFML libraries") + import fnmatch if cfg.options.sitl_osd: - env.CXXFLAGS += ['-DWITH_SITL_OSD','-DOSD_ENABLED=ENABLED','-DHAL_HAVE_AP_ROMFS_EMBEDDED_H'] - import fnmatch + env.CXXFLAGS += ['-DWITH_SITL_OSD','-DOSD_ENABLED=1'] for f in os.listdir('libraries/AP_OSD/fonts'): if fnmatch.fnmatch(f, "font*bin"): env.ROMFS_FILES += [(f,'libraries/AP_OSD/fonts/'+f)] + # embed any scripts from ROMFS/scripts + if os.path.exists('ROMFS/scripts'): + for f in os.listdir('ROMFS/scripts'): + if fnmatch.fnmatch(f, "*.lua"): + env.ROMFS_FILES += [('scripts/'+f,'ROMFS/scripts/'+f)] + + if len(env.ROMFS_FILES) > 0: + env.CXXFLAGS += ['-DHAL_HAVE_AP_ROMFS_EMBEDDED_H'] + if cfg.options.sitl_rgbled: env.CXXFLAGS += ['-DWITH_SITL_RGBLED'] @@ -571,12 +580,12 @@ def build(self, bld): def pre_build(self, bld): '''pre-build hook that gets called before dynamic sources''' - super(chibios, self).pre_build(bld) from waflib.Context import load_tool module = load_tool('chibios', [], with_sys_path=True) fun = getattr(module, 'pre_build', None) if fun: fun(bld) + super(chibios, self).pre_build(bld) class linux(Board): def configure_env(self, cfg, env): diff --git a/Tools/ardupilotwaf/chibios.py b/Tools/ardupilotwaf/chibios.py index 6ee51841f22fc..8c89578306373 100644 --- a/Tools/ardupilotwaf/chibios.py +++ b/Tools/ardupilotwaf/chibios.py @@ -352,38 +352,50 @@ def bldpath(path): cfg.msg('Default parameters', cfg.options.default_parameters, color='YELLOW') env.DEFAULT_PARAMETERS = cfg.options.default_parameters - # we need to run chibios_hwdef.py at configure stage to generate the ldscript.ld - # that is needed by the remaining configure checks + try: + ret = generate_hwdef_h(env) + except Exception: + cfg.fatal("Failed to process hwdef.dat") + if ret != 0: + cfg.fatal("Failed to process hwdef.dat ret=%d" % ret) + load_env_vars(cfg.env) + if env.HAL_WITH_UAVCAN: + setup_can_build(cfg) + setup_optimization(cfg.env) + +def generate_hwdef_h(env): + '''run chibios_hwdef.py''' import subprocess if env.BOOTLOADER: - env.HWDEF = srcpath('libraries/AP_HAL_ChibiOS/hwdef/%s/hwdef-bl.dat' % env.BOARD) + env.HWDEF = os.path.join(env.SRCROOT, 'libraries/AP_HAL_ChibiOS/hwdef/%s/hwdef-bl.dat' % env.BOARD) env.BOOTLOADER_OPTION="--bootloader" else: - env.HWDEF = srcpath('libraries/AP_HAL_ChibiOS/hwdef/%s/hwdef.dat' % env.BOARD) + env.HWDEF = os.path.join(env.SRCROOT, 'libraries/AP_HAL_ChibiOS/hwdef/%s/hwdef.dat' % env.BOARD) env.BOOTLOADER_OPTION="" - hwdef_script = srcpath('libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py') + hwdef_script = os.path.join(env.SRCROOT, 'libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py') hwdef_out = env.BUILDROOT if not os.path.exists(hwdef_out): os.mkdir(hwdef_out) python = sys.executable - try: - cmd = "{0} '{1}' -D '{2}' '{3}' {4} --params '{5}'".format(python, hwdef_script, hwdef_out, env.HWDEF, env.BOOTLOADER_OPTION, cfg.options.default_parameters) - ret = subprocess.call(cmd, shell=True) - except Exception: - cfg.fatal("Failed to process hwdef.dat") - if ret != 0: - cfg.fatal("Failed to process hwdef.dat ret=%d" % ret) - load_env_vars(cfg.env) - if env.HAL_WITH_UAVCAN: - setup_can_build(cfg) - setup_optimization(cfg.env) + cmd = "{0} '{1}' -D '{2}' '{3}' {4} --params '{5}'".format(python, hwdef_script, hwdef_out, env.HWDEF, env.BOOTLOADER_OPTION, env.DEFAULT_PARAMETERS) + return subprocess.call(cmd, shell=True) def pre_build(bld): '''pre-build hook to change dynamic sources''' load_env_vars(bld.env) if bld.env.HAL_WITH_UAVCAN: bld.get_board().with_uavcan = True + hwdef_h = os.path.join(bld.env.BUILDROOT, 'hwdef.h') + if not os.path.exists(hwdef_h): + print("Generating hwdef.h") + try: + ret = generate_hwdef_h(bld.env) + except Exception: + bld.fatal("Failed to process hwdef.dat") + if ret != 0: + bld.fatal("Failed to process hwdef.dat ret=%d" % ret) + setup_optimization(bld.env) def build(bld): @@ -394,7 +406,8 @@ def build(bld): bld.env.get_flat('PYTHON'), bld.env.HWDEF, bld.env.BOOTLOADER_OPTION, bld.env.default_parameters), group='dynamic_sources', target=[bld.bldnode.find_or_declare('hwdef.h'), - bld.bldnode.find_or_declare('ldscript.ld')] + bld.bldnode.find_or_declare('ldscript.ld'), + bld.bldnode.find_or_declare('hw.dat')] ) bld( @@ -405,6 +418,7 @@ def build(bld): ) common_src = [bld.bldnode.find_or_declare('hwdef.h'), + bld.bldnode.find_or_declare('hw.dat'), bld.bldnode.find_or_declare('modules/ChibiOS/include_dirs')] common_src += bld.path.ant_glob('libraries/AP_HAL_ChibiOS/hwdef/common/*.[ch]') common_src += bld.path.ant_glob('libraries/AP_HAL_ChibiOS/hwdef/common/*.mk') diff --git a/Tools/ardupilotwaf/embed.py b/Tools/ardupilotwaf/embed.py index daae8c00faf2a..160d16b5c222b 100644 --- a/Tools/ardupilotwaf/embed.py +++ b/Tools/ardupilotwaf/embed.py @@ -68,6 +68,9 @@ def create_embedded_h(filename, files, uncompressed=False): out = open(filename, "wb") write_encode(out, '''// generated embedded files for AP_ROMFS\n\n''') + # remove duplicates and sort + files = sorted(list(set(files))) + for i in range(len(files)): (name, filename) = files[i] if not embed_file(out, filename, i, name, uncompressed): diff --git a/libraries/AP_Filesystem/AP_Filesystem.cpp b/libraries/AP_Filesystem/AP_Filesystem.cpp index fb7b816f3fd6c..1c5c6423e7010 100644 --- a/libraries/AP_Filesystem/AP_Filesystem.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem.cpp @@ -19,6 +19,190 @@ 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*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 { AP_Filesystem &FS() diff --git a/libraries/AP_Filesystem/AP_Filesystem.h b/libraries/AP_Filesystem/AP_Filesystem.h index fb63fed4eed97..b37260ca54a3d 100644 --- a/libraries/AP_Filesystem/AP_Filesystem.h +++ b/libraries/AP_Filesystem/AP_Filesystem.h @@ -33,6 +33,11 @@ #endif class AP_Filesystem { +private: + struct DirHandle { + uint8_t fs_index; + void *dir; + }; public: AP_Filesystem() {} @@ -47,9 +52,10 @@ class AP_Filesystem { 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); + + DirHandle *opendir(const char *pathname); + struct dirent *readdir(DirHandle *dirp); + int closedir(DirHandle *dirp); // return free disk space in bytes, -1 on error int64_t disk_free(const char *path); @@ -59,6 +65,23 @@ class AP_Filesystem { // set modification time on a file 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 { diff --git a/libraries/AP_Filesystem/AP_Filesystem_FATFS.cpp b/libraries/AP_Filesystem/AP_Filesystem_FATFS.cpp index 1dfec099ed7b7..4c080ffd1bb20 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_FATFS.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem_FATFS.cpp @@ -276,7 +276,7 @@ static bool remount_file_system(void) return true; } -int AP_Filesystem::open(const char *pathname, int flags) +int AP_Filesystem_FATFS::open(const char *pathname, int flags) { int fileno; int fatfs_modes; @@ -352,7 +352,7 @@ int AP_Filesystem::open(const char *pathname, int flags) return fileno; } -int AP_Filesystem::close(int fileno) +int AP_Filesystem_FATFS::close(int fileno) { FAT_FILE *stream; FIL *fh; @@ -382,7 +382,7 @@ int AP_Filesystem::close(int fileno) 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; int res; @@ -431,7 +431,7 @@ ssize_t AP_Filesystem::read(int fd, void *buf, size_t count) 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; FRESULT res; @@ -479,7 +479,7 @@ ssize_t AP_Filesystem::write(int fd, const void *buf, size_t count) return (ssize_t)total; } -int AP_Filesystem::fsync(int fileno) +int AP_Filesystem_FATFS::fsync(int fileno) { FAT_FILE *stream; FIL *fh; @@ -508,7 +508,7 @@ int AP_Filesystem::fsync(int fileno) 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; FIL *fh; @@ -599,7 +599,7 @@ static time_t fat_time_to_unix(uint16_t date, uint16_t time) 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; int res; @@ -667,7 +667,7 @@ int AP_Filesystem::stat(const char *name, struct stat *buf) return 0; } -int AP_Filesystem::unlink(const char *pathname) +int AP_Filesystem_FATFS::unlink(const char *pathname) { WITH_SEMAPHORE(sem); @@ -680,7 +680,7 @@ int AP_Filesystem::unlink(const char *pathname) return 0; } -int AP_Filesystem::mkdir(const char *pathname) +int AP_Filesystem_FATFS::mkdir(const char *pathname) { WITH_SEMAPHORE(sem); @@ -703,7 +703,7 @@ struct DIR_Wrapper { struct dirent de; }; -DIR *AP_Filesystem::opendir(const char *pathdir) +void *AP_Filesystem_FATFS::opendir(const char *pathdir) { WITH_SEMAPHORE(sem); @@ -730,9 +730,10 @@ DIR *AP_Filesystem::opendir(const char *pathdir) return &ret->d; } -struct dirent *AP_Filesystem::readdir(DIR *dirp) +struct dirent *AP_Filesystem_FATFS::readdir(void *dirp_void) { WITH_SEMAPHORE(sem); + DIR *dirp = (DIR *)dirp_void; struct DIR_Wrapper *d = (struct DIR_Wrapper *)dirp; if (!d) { @@ -744,7 +745,7 @@ struct dirent *AP_Filesystem::readdir(DIR *dirp) int res; d->de.d_name[0] = 0; - res = f_readdir ( dirp, &fno ); + res = f_readdir(dirp, &fno); if (res != FR_OK || fno.fname[0] == 0) { errno = fatfs_to_errno((FRESULT)res); return nullptr; @@ -760,8 +761,9 @@ struct dirent *AP_Filesystem::readdir(DIR *dirp) 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); struct DIR_Wrapper *d = (struct DIR_Wrapper *)dirp; @@ -780,7 +782,7 @@ int AP_Filesystem::closedir(DIR *dirp) } // 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); @@ -801,7 +803,7 @@ int64_t AP_Filesystem::disk_free(const char *path) } // 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); @@ -841,7 +843,7 @@ static void unix_time_to_fat(time_t epoch, uint16_t &date, uint16_t &time) /* 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; uint16_t fdate, ftime; diff --git a/libraries/AP_Filesystem/AP_Filesystem_FATFS.h b/libraries/AP_Filesystem/AP_Filesystem_FATFS.h index d047dbf768842..5844dfc38084a 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_FATFS.h +++ b/libraries/AP_Filesystem/AP_Filesystem_FATFS.h @@ -2,12 +2,15 @@ FATFS backend for AP_Filesystem */ +#pragma once + #include #include #include #include #include #include +#include "AP_Filesystem_backend.h" // Seek offset macros #define SEEK_SET 0 @@ -27,3 +30,30 @@ struct dirent { char d_name[MAX_NAME_LEN]; /* filename */ 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; +}; diff --git a/libraries/AP_Filesystem/AP_Filesystem_ROMFS.cpp b/libraries/AP_Filesystem/AP_Filesystem_ROMFS.cpp new file mode 100644 index 0000000000000..5402232cebf82 --- /dev/null +++ b/libraries/AP_Filesystem/AP_Filesystem_ROMFS.cpp @@ -0,0 +1,211 @@ +/* + 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 ROMFS + */ +#include "AP_Filesystem.h" +#include "AP_Filesystem_ROMFS.h" +#include +#include +#include + +#if defined(HAL_HAVE_AP_ROMFS_EMBEDDED_H) && HAVE_FILESYSTEM_SUPPORT + +int AP_Filesystem_ROMFS::open(const char *fname, int flags) +{ + if ((flags & O_ACCMODE) != O_RDONLY) { + errno = EROFS; + return -1; + } + uint8_t idx; + for (idx=0; idx= 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) { + 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 diff --git a/libraries/AP_Filesystem/AP_Filesystem_ROMFS.h b/libraries/AP_Filesystem/AP_Filesystem_ROMFS.h new file mode 100644 index 0000000000000..9e202f38a2da0 --- /dev/null +++ b/libraries/AP_Filesystem/AP_Filesystem_ROMFS.h @@ -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 . + */ + +#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 diff --git a/libraries/AP_Filesystem/AP_Filesystem_backend.h b/libraries/AP_Filesystem/AP_Filesystem_backend.h new file mode 100644 index 0000000000000..0134c525bb338 --- /dev/null +++ b/libraries/AP_Filesystem/AP_Filesystem_backend.h @@ -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 . + */ +/* + ArduPilot filesystem backend interface. + */ +#pragma once + +#include +#include + +#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 diff --git a/libraries/AP_Filesystem/AP_Filesystem_posix.cpp b/libraries/AP_Filesystem/AP_Filesystem_posix.cpp index 042bcf2db346d..ad1c69f216c57 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_posix.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem_posix.cpp @@ -48,45 +48,45 @@ static const char *map_filename(const char *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); // 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) +int AP_Filesystem_Posix::close(int 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); } -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); } -int AP_Filesystem::fsync(int fd) +int AP_Filesystem_Posix::fsync(int 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); } -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); return ::stat(pathname, stbuf); } -int AP_Filesystem::unlink(const char *pathname) +int AP_Filesystem_Posix::unlink(const char *pathname) { pathname = map_filename(pathname); // we match the FATFS interface and use unlink @@ -98,30 +98,30 @@ int AP_Filesystem::unlink(const char *pathname) return ret; } -int AP_Filesystem::mkdir(const char *pathname) +int AP_Filesystem_Posix::mkdir(const char *pathname) { pathname = map_filename(pathname); return ::mkdir(pathname, 0775); } -DIR *AP_Filesystem::opendir(const char *pathname) +void *AP_Filesystem_Posix::opendir(const char *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 -int64_t AP_Filesystem::disk_free(const char *path) +int64_t AP_Filesystem_Posix::disk_free(const char *path) { path = map_filename(path); struct statfs stats; @@ -132,7 +132,7 @@ int64_t AP_Filesystem::disk_free(const char *path) } // 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); struct statfs stats; @@ -146,7 +146,7 @@ int64_t AP_Filesystem::disk_space(const char *path) /* 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); struct utimbuf times {}; diff --git a/libraries/AP_Filesystem/AP_Filesystem_posix.h b/libraries/AP_Filesystem/AP_Filesystem_posix.h index 1f5f8bd2a8253..75c0c4a711741 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_posix.h +++ b/libraries/AP_Filesystem/AP_Filesystem_posix.h @@ -23,3 +23,32 @@ #include #include #include +#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; +}; + diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c b/libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c index 543df832a260a..a266f9c3d89c5 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c @@ -287,3 +287,18 @@ void memory_flush_all(void) stm32_cacheBufferFlush(memory_regions[i].address, memory_regions[i].size); } } + +/* + replacement for strdup + */ +char *strdup(const char *str) +{ + const size_t len = strlen(str); + char *ret = malloc(len+1); + if (!ret) { + return NULL; + } + memcpy(ret, str, len); + ret[len] = 0; + return ret; +} diff --git a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py index a5d05d78d1cc4..939a600860c40 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py +++ b/libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py @@ -97,6 +97,8 @@ compass_list = [] baro_list = [] +all_lines = [] + mcu_type = None dual_USB_enabled = False @@ -1586,6 +1588,13 @@ def write_alt_config(f): f.write(" { %u, %s, PAL_LINE(GPIO%s,%uU)}, /* %s */ \\\n" % (alt, p.pal_modeline(), p.port, p.pin, str(p))) f.write('}\n\n') +def write_all_lines(hwdat): + f = open(hwdat, 'w') + f.write('\n'.join(all_lines)) + f.close() + flash_size = get_config('FLASH_SIZE_KB', type=int) + if flash_size > 1024: + romfs["hwdef.dat"] = hwdat def write_hwdef_header(outfilename): '''write hwdef header file''' @@ -1815,11 +1824,25 @@ def romfs_wildcard(pattern): if fnmatch.fnmatch(f, pattern): romfs[f] = os.path.join(pattern_dir, f) +def romfs_add_dir(subdirs): + '''add a filesystem directory to ROMFS''' + for dirname in subdirs: + romfs_dir = os.path.join(os.path.dirname(args.hwdef), dirname) + if not args.bootloader and os.path.exists(romfs_dir): + for root, d, files in os.walk(romfs_dir): + for f in files: + if fnmatch.fnmatch(f, '*~'): + # skip editor backup files + continue + fullpath = os.path.join(root, f) + relpath = os.path.normpath(os.path.join(dirname, os.path.relpath(root, romfs_dir), f)) + romfs[relpath] = fullpath def process_line(line): '''process one line of pin definition file''' global allpins, imu_list, compass_list, baro_list global mcu_type, mcu_series + all_lines.append(line) a = shlex.split(line) # keep all config lines for later use alllines.append(line) @@ -1960,12 +1983,17 @@ def process_file(filename): # build a list for peripherals for DMA resolver periph_list = build_peripheral_list() +# write out hw.dat for ROMFS +write_all_lines(os.path.join(outdir, "hw.dat")) + # write out hwdef.h write_hwdef_header(os.path.join(outdir, "hwdef.h")) # write out ldscript.ld write_ldscript(os.path.join(outdir, "ldscript.ld")) +romfs_add_dir(['scripts']) + write_ROMFS(outdir) # copy the shared linker script into the build directory; it must diff --git a/libraries/AP_Logger/AP_Logger_File.cpp b/libraries/AP_Logger/AP_Logger_File.cpp index b44bc285203f8..531b2c247f425 100644 --- a/libraries/AP_Logger/AP_Logger_File.cpp +++ b/libraries/AP_Logger/AP_Logger_File.cpp @@ -225,7 +225,7 @@ uint16_t AP_Logger_File::find_oldest_log() // relying on the min_avail_space_percent feature we could end up // doing a *lot* of asprintf()s and stat()s EXPECT_DELAY_MS(3000); - DIR *d = AP::FS().opendir(_log_directory); + auto *d = AP::FS().opendir(_log_directory); if (d == nullptr) { // SD card may have died? On linux someone may have rm-rf-d return 0; diff --git a/libraries/AP_OSD/AP_OSD.cpp b/libraries/AP_OSD/AP_OSD.cpp index 14f3f385e753a..925e466cf579e 100644 --- a/libraries/AP_OSD/AP_OSD.cpp +++ b/libraries/AP_OSD/AP_OSD.cpp @@ -18,6 +18,9 @@ */ #include "AP_OSD.h" + +#if OSD_ENABLED + #include "AP_OSD_MAX7456.h" #ifdef WITH_SITL_OSD #include "AP_OSD_SITL.h" @@ -400,3 +403,5 @@ void AP_OSD::set_nav_info(NavInfo &navinfo) AP_OSD *AP::osd() { return AP_OSD::get_singleton(); } + +#endif // OSD_ENABLED diff --git a/libraries/AP_Param/AP_Param.h b/libraries/AP_Param/AP_Param.h index a39543e463214..4e60dfe18855e 100644 --- a/libraries/AP_Param/AP_Param.h +++ b/libraries/AP_Param/AP_Param.h @@ -39,7 +39,7 @@ maximum size of embedded parameter file */ #ifndef AP_PARAM_MAX_EMBEDDED_PARAM -#if HAL_MINIMIZE_FEATURES +#if BOARD_FLASH_SIZE <= 1024 # define AP_PARAM_MAX_EMBEDDED_PARAM 1024 #else # define AP_PARAM_MAX_EMBEDDED_PARAM 8192 diff --git a/libraries/AP_ROMFS/AP_ROMFS.cpp b/libraries/AP_ROMFS/AP_ROMFS.cpp index 9f6a05097b0bc..f87915e520d98 100644 --- a/libraries/AP_ROMFS/AP_ROMFS.cpp +++ b/libraries/AP_ROMFS/AP_ROMFS.cpp @@ -113,3 +113,21 @@ void AP_ROMFS::free(const uint8_t *data) ::free(const_cast(data)); #endif } + +/* + directory listing interface. Start with ofs=0. Returns pathnames + that match dirname prefix. Ends with nullptr return when no more + files found +*/ +const char *AP_ROMFS::dir_list(const char *dirname, uint16_t &ofs) +{ + const size_t dlen = strlen(dirname); + for ( ; ofs < ARRAY_SIZE(files); ofs++) { + if (strncmp(dirname, files[ofs].filename, dlen) == 0 && + files[ofs].filename[dlen] == '/') { + // found one + return files[ofs++].filename; + } + } + return nullptr; +} diff --git a/libraries/AP_ROMFS/AP_ROMFS.h b/libraries/AP_ROMFS/AP_ROMFS.h index 213f033d248ee..4dc6458b5c47f 100644 --- a/libraries/AP_ROMFS/AP_ROMFS.h +++ b/libraries/AP_ROMFS/AP_ROMFS.h @@ -16,6 +16,13 @@ class AP_ROMFS { // free returned data static void free(const uint8_t *data); + /* + directory listing interface. Start with ofs=0. Returns pathnames + that match dirname prefix. Ends with nullptr return when no more + files found + */ + static const char *dir_list(const char *dirname, uint16_t &ofs); + private: // find an embedded file static const uint8_t *find_file(const char *name, uint32_t &size); diff --git a/libraries/AP_Scripting/lua_scripts.cpp b/libraries/AP_Scripting/lua_scripts.cpp index f93c4bb773c42..ae44cf028594d 100644 --- a/libraries/AP_Scripting/lua_scripts.cpp +++ b/libraries/AP_Scripting/lua_scripts.cpp @@ -130,7 +130,7 @@ void lua_scripts::load_all_scripts_in_dir(lua_State *L, const char *dirname) { return; } - DIR *d = AP::FS().opendir(dirname); + auto *d = AP::FS().opendir(dirname); if (d == nullptr) { gcs().send_text(MAV_SEVERITY_INFO, "Lua: open directory (%s) failed", dirname); return; @@ -372,6 +372,7 @@ void lua_scripts::run(void) { // Scan the filesystem in an appropriate manner and autostart scripts load_all_scripts_in_dir(L, SCRIPTING_DIRECTORY); + load_all_scripts_in_dir(L, "@ROMFS/scripts"); succeeded_initial_load = true; diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h index 49eb6d006a29f..0322446e984e3 100644 --- a/libraries/GCS_MAVLink/GCS.h +++ b/libraries/GCS_MAVLink/GCS.h @@ -707,6 +707,7 @@ class GCS_MAVLINK int fd = -1; FTP_FILE_MODE mode; // work around AP_Filesystem not supporting file modes int16_t current_session; + uint32_t last_send_ms; }; static struct ftp_state ftp; diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 48c19a01accb4..28eb2945f91da 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -847,6 +847,12 @@ uint16_t GCS_MAVLINK::get_reschedule_interval_ms(const deferred_message_bucket_t // we are sending requests for waypoints, penalize streams: interval_ms *= 4; } +#if HAVE_FILESYSTEM_SUPPORT + if (ftp.replies && AP_HAL::millis() - ftp.last_send_ms < 500) { + // we are sending ftp replies + interval_ms *= 4; + } +#endif if (interval_ms > 60000) { return 60000; diff --git a/libraries/GCS_MAVLink/GCS_FTP.cpp b/libraries/GCS_MAVLink/GCS_FTP.cpp index 2e2f44aa279cf..9139711986fbf 100644 --- a/libraries/GCS_MAVLink/GCS_FTP.cpp +++ b/libraries/GCS_MAVLink/GCS_FTP.cpp @@ -114,6 +114,7 @@ void GCS_MAVLINK::send_ftp_replies(void) { 0, reply.sysid, reply.compid, payload); ftp.replies->pop(reply); + ftp.last_send_ms = AP_HAL::millis(); } else { return; } @@ -147,7 +148,7 @@ void GCS_MAVLINK::ftp_error(struct pending_ftp &response, FTP_ERROR error) { void GCS_MAVLINK::ftp_push_replies(pending_ftp &reply) { while (!ftp.replies->push(reply)) { // we must fit the response, keep shoving it in - hal.scheduler->delay(10); + hal.scheduler->delay(2); } } @@ -159,7 +160,7 @@ void GCS_MAVLINK::ftp_worker(void) { while (true) { while (!ftp.requests->pop(request)) { // nothing to handle, delay ourselves a bit then check again. Ideally we'd use conditional waits here - hal.scheduler->delay(10); + hal.scheduler->delay(2); } // if it's a rerequest and we still have the last response then send it @@ -436,6 +437,7 @@ void GCS_MAVLINK::ftp_worker(void) { } case FTP_OP::BurstReadFile: { + const uint16_t max_read = (request.size == 0?sizeof(reply.data):request.size); // must actually be working on a file if (ftp.fd == -1) { ftp_error(reply, FTP_ERROR::FileNotFound); @@ -458,7 +460,7 @@ void GCS_MAVLINK::ftp_worker(void) { const uint32_t transfer_size = 100; for (uint32_t i = 0; (i < transfer_size) && more_pending; i++) { // fill the buffer - const ssize_t read_bytes = AP::FS().read(ftp.fd, reply.data, sizeof(reply.data)); + const ssize_t read_bytes = AP::FS().read(ftp.fd, reply.data, max_read); if (read_bytes == -1) { ftp_error(reply, FTP_ERROR::FailErrno); more_pending = false; @@ -477,7 +479,7 @@ void GCS_MAVLINK::ftp_worker(void) { } reply.opcode = FTP_OP::Ack; - reply.offset = request.offset + i * sizeof(reply.data); + reply.offset = request.offset + i * max_read; reply.burst_complete = (i == (transfer_size - 1)); reply.size = (uint8_t)read_bytes; @@ -545,10 +547,9 @@ void GCS_MAVLINK::ftp_list_dir(struct pending_ftp &request, struct pending_ftp & request.data[sizeof(request.data) - 1] = 0; // ensure the path is null terminated // open the dir - DIR *dir = AP::FS().opendir((char *)request.data); + auto *dir = AP::FS().opendir((char *)request.data); if (dir == nullptr) { ftp_error(response, FTP_ERROR::FailErrno); - AP::FS().closedir(dir); return; }