diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index 765f3be88e2d5b..3f88fcd1c52a80 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -433,6 +433,7 @@ def config_option(self): Feature('Networking', 'PPP', 'AP_NETWORKING_BACKEND_PPP', 'Enable PPP networking', 0, None), # Feature('Networking', 'CAN MCAST', 'AP_NETWORKING_CAN_MCAST_ENABLED', 'Enable CAN multicast bridge', 0, None), + Feature('Networking', '9P2000', 'AP_NETWORKING_FILESYSTEM_ENABLED', 'Enable 9P2000 newtwork filesystem client', 0, None), Feature('CAN', 'DroneCAN', 'HAL_ENABLE_DRONECAN_DRIVERS', 'Enable DroneCAN support', 0, None), Feature('CAN', 'CAN Logging', 'AP_CAN_LOGGING_ENABLED', 'Enable CAN logging support', 0, 'Logging'), diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py index 7ed2a136ddd329..b7cb24ddfefb3c 100755 --- a/Tools/scripts/extract_features.py +++ b/Tools/scripts/extract_features.py @@ -264,8 +264,11 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"): ('AP_TUNING_ENABLED', 'AP_Tuning::check_input'), ('AP_DRONECAN_SERIAL_ENABLED', 'AP_DroneCAN_Serial::update'), ('AP_SERIALMANAGER_IMUOUT_ENABLED', 'AP_InertialSensor::send_uart_data'), + ('AP_NETWORKING_BACKEND_PPP', 'AP_Networking_PPP::init'), ('AP_NETWORKING_CAN_MCAST_ENABLED', 'AP_Networking_CAN::start'), + ('AP_NETWORKING_FILESYSTEM_ENABLED', r'AP_Networking::NineP2000::init'), + ('FORCE_APJ_DEFAULT_PARAMETERS', 'AP_Param::param_defaults_data'), ('HAL_BUTTON_ENABLED', 'AP_Button::update'), ('HAL_LOGGING_ENABLED', 'AP_Logger::init'), diff --git a/libraries/AP_Filesystem/AP_Filesystem.cpp b/libraries/AP_Filesystem/AP_Filesystem.cpp index 63fdbee0556b08..ee3cdbd5feec95 100644 --- a/libraries/AP_Filesystem/AP_Filesystem.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem.cpp @@ -60,6 +60,11 @@ static AP_Filesystem_Sys fs_sys; static AP_Filesystem_Mission fs_mission; #endif +#include "AP_Filesystem_9P2000.h" +#if AP_NETWORKING_FILESYSTEM_ENABLED +static AP_Filesystem_9P2000 fs_9P2000; +#endif + /* mapping from filesystem prefix to backend */ @@ -77,6 +82,9 @@ const AP_Filesystem::Backend AP_Filesystem::backends[] = { #if AP_FILESYSTEM_MISSION_ENABLED { "@MISSION", fs_mission }, #endif +#if AP_NETWORKING_FILESYSTEM_ENABLED + { "@9P2000", fs_9P2000 }, +#endif }; extern const AP_HAL::HAL& hal; diff --git a/libraries/AP_Filesystem/AP_Filesystem_9P2000.cpp b/libraries/AP_Filesystem/AP_Filesystem_9P2000.cpp new file mode 100644 index 00000000000000..dead555042ed3f --- /dev/null +++ b/libraries/AP_Filesystem/AP_Filesystem_9P2000.cpp @@ -0,0 +1,730 @@ +/* + 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 + +#if AP_NETWORKING_FILESYSTEM_ENABLED + +#include "AP_Filesystem.h" +#include "AP_Filesystem_9P2000.h" +#include +#include + +extern const AP_HAL::HAL& hal; + +#ifdef ENOTBLK + #define MAIN_THREAD_ERROR ENOTBLK; +#else + #define MAIN_THREAD_ERROR EPERM; +#endif + +// Wait for response, blocking +bool AP_Filesystem_9P2000::wait_for_tag(NineP2000& fs, const uint16_t tag) const +{ + // Tag is invalid + if (tag == fs.NOTAG) { + errno = ENFILE; + return false; + } + + const uint32_t timeout = 250; + const uint32_t wait_start = AP_HAL::millis(); + do { + if (fs.tag_response(tag)) { + return true; + } + hal.scheduler->delay(1); + } while ((AP_HAL::millis() - wait_start) < timeout); + + // Timeout! + fs.clear_tag(tag); + errno = EBUSY; + return false; +} + +// Open a given file ID with flags +bool AP_Filesystem_9P2000::open_fileId(NineP2000& fs, const uint32_t fileId, const int flags) +{ + // Request the open + const uint16_t tag = fs.request_open(fileId, flags); + + // Wait for the reply + if (!wait_for_tag(fs, tag)) { + return false; + } + + // Got reply + return fs.open_result(tag); +} + +// Get file id for a given path, return 0 if fail +uint32_t AP_Filesystem_9P2000::get_file_id(NineP2000& fs, const char *name, const NineP2000::walkType type) const +{ + // Navigate to the given path + const uint16_t tag = fs.request_walk(name, type); + + // Wait for the reply + if (!wait_for_tag(fs, tag)) { + return 0; + } + + // Grab reply, should be none zero + return fs.walk_result(tag); +} + +bool AP_Filesystem_9P2000::create_file(NineP2000& fs, const char *fname, bool is_dir) +{ + // Copy string a split into path and name + const uint16_t len = strlen(fname); + + uint16_t split = 0; + bool found = false; + for (uint16_t i = 0; i < len; i++) { + if (fname[i] == '/') { + found = true; + split = i; + } + } + + char name[len + 1]; + memset(&name, 0, sizeof(name)); + memcpy(&name, fname, len); + if (found) { + name[split] = 0; + } + + // Navigate to parent directory + const uint32_t fid = get_file_id(fs, found ? name : "", NineP2000::walkType::Directory); + if (fid == 0) { + return false; + } + + // Create the new item + const uint8_t name_start = split + (found ? 1 : 0); + const uint16_t tag = fs.request_create(fid, &name[name_start], is_dir); + if (tag == fs.NOTAG) { + return false; + } + + // Wait for the reply + if (!wait_for_tag(fs, tag)) { + fs.free_file_id(fid); + return false; + } + + const bool ret = fs.create_result(tag); + + fs.free_file_id(fid); + + return ret; +} + +int AP_Filesystem_9P2000::open(const char *fname, int flags, bool allow_absolute_paths) +{ + if (hal.scheduler->in_main_thread()) { + // Too slow for the main thread + errno = MAIN_THREAD_ERROR; + return -1; + } + + // Make sure filesystem is mounted. + NineP2000& fs = AP::network().get_filesystem(); + if (!fs.mounted()) { + errno = ENODEV; + return -1; + } + + // Find free file object + uint8_t idx; + for (idx=0; idxin_main_thread()) { + // Too slow for the main thread + errno = MAIN_THREAD_ERROR; + return -1; + } + + // Check valid file + if ((fd < 0) || (fd >= (int)ARRAY_SIZE(file)) || (file[fd].fileId == 0)) { + errno = EBADF; + return -1; + } + + // Make sure filesystem is mounted. + NineP2000& fs = AP::network().get_filesystem(); + if (!fs.mounted()) { + errno = ENODEV; + return -1; + } + + // Free ID and clear file object for reused + fs.free_file_id(file[fd].fileId); + file[fd].fileId = 0; + return 0; +} + +int32_t AP_Filesystem_9P2000::read(int fd, void *buf, uint32_t count) +{ + if (hal.scheduler->in_main_thread()) { + // Too slow for the main thread + errno = MAIN_THREAD_ERROR; + return -1; + } + + if ((fd < 0) || (fd >= (int)ARRAY_SIZE(file)) || (file[fd].fileId == 0)) { + // Invalid file + errno = EBADF; + return -1; + } + + // Make sure filesystem is mounted. + NineP2000& fs = AP::network().get_filesystem(); + if (!fs.mounted()) { + errno = ENODEV; + return -1; + } + + // Don't try and read too much in one go + const uint32_t max_read = fs.max_read_len(); + + uint32_t total = 0; + do { + const uint32_t read_count = MIN(count - total, max_read); + + // Send read command + const uint16_t tag = fs.request_file_read(file[fd].fileId, file[fd].ofs, read_count, buf); + + // Wait for the reply + if (!wait_for_tag(fs, tag)) { + // May have been able to do a partial read + if (total > 0) { + break; + } + return -1; + } + + const int read = fs.read_result(tag, false); + if (read <= 0) { + if (total > 0) { + break; + } + return -1; + } + + file[fd].ofs += read; + total += read; + + // failed to read the expected amount, don't try again + if ((uint32_t)read != read_count) { + break; + } + + // Increment the buffer + buf = (void *)(((uint8_t *)buf) + read); + + } while (count > total); + + return total; +} + +int32_t AP_Filesystem_9P2000::write(int fd, const void *buf, uint32_t count) +{ + if (hal.scheduler->in_main_thread()) { + // Too slow for the main thread + errno = MAIN_THREAD_ERROR; + return -1; + } + + if ((fd < 0) || (fd >= (int)ARRAY_SIZE(file)) || (file[fd].fileId == 0)) { + // Invalid file + errno = EBADF; + return -1; + } + + // Make sure filesystem is mounted. + NineP2000& fs = AP::network().get_filesystem(); + if (!fs.mounted()) { + errno = ENODEV; + return -1; + } + + // Don't try and write too much in one go + const uint32_t max_write = fs.max_write_len(); + + uint32_t total = 0; + do { + const uint32_t write_count = MIN(count - total, max_write); + + // Send write command + const uint16_t tag = fs.request_write(file[fd].fileId, file[fd].ofs, write_count, buf); + + // Wait for the reply + if (!wait_for_tag(fs, tag)) { + // May have been able to do a partial write + if (total > 0) { + break; + } + return -1; + } + + const int32_t written = fs.write_result(tag); + if (written <= 0) { + if (total > 0) { + break; + } + return -1; + } + + // Increment the file offset and size + file[fd].ofs += written; + file[fd].size = MAX(file[fd].size, file[fd].ofs); + total += written; + + // failed to write the expected amount, don't try again + if ((uint32_t)written != write_count) { + break; + } + + // Increment the buffer + buf = (void *)(((uint8_t *)buf) + written); + + } while (count > total); + + // Return length written + return total; +} + +int32_t AP_Filesystem_9P2000::lseek(int fd, int32_t offset, int seek_from) +{ + if ((fd < 0) || (fd >= (int)ARRAY_SIZE(file)) || (file[fd].fileId == 0)) { + // Invalid file + errno = EBADF; + return -1; + } + + // Just move the local copy of the offset, there is no 9P2000 command for seek + // This means that the next read starts in the correct spot + switch (seek_from) { + case SEEK_SET: + if (offset < 0) { + errno = EINVAL; + return -1; + } + file[fd].ofs = MIN(file[fd].size, (uint32_t)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_9P2000::stat(const char *name, struct stat *stbuf) +{ + if (hal.scheduler->in_main_thread()) { + // Too slow for the main thread + errno = MAIN_THREAD_ERROR; + return -1; + } + + // Make sure filesystem is mounted. + NineP2000& fs = AP::network().get_filesystem(); + if (!fs.mounted()) { + errno = ENODEV; + return -1; + } + + // Navigate to the given path + const uint32_t fid = get_file_id(fs, name, NineP2000::walkType::File); + if (fid == 0) { + errno = ENOENT; + return -1; + } + + // Request stat + const uint16_t tag = fs.request_stat(fid, stbuf); + if (tag == fs.NOTAG) { + return -1; + } + + // Wait for the reply + if (!wait_for_tag(fs, tag)) { + fs.free_file_id(fid); + return -1; + } + + // Get result + const int ret = fs.stat_result(tag) ? 0 : -1; + + // Return file handle + fs.free_file_id(fid); + + return ret; +} + +int AP_Filesystem_9P2000::unlink(const char *pathname) +{ + if (hal.scheduler->in_main_thread()) { + // Too slow for the main thread + errno = MAIN_THREAD_ERROR; + return -1; + } + + // Make sure filesystem is mounted. + NineP2000& fs = AP::network().get_filesystem(); + if (!fs.mounted()) { + errno = ENODEV; + return -1; + } + + // Navigate to the given path + const uint32_t fid = get_file_id(fs, pathname, NineP2000::walkType::Any); + if (fid == 0) { + errno = ENOENT; + return -1; + } + + // Request stat + const uint16_t tag = fs.request_remove(fid); + if (tag == fs.NOTAG) { + return -1; + } + + // Wait for the reply + if (!wait_for_tag(fs, tag)) { + fs.free_file_id(fid); + return -1; + } + + // Get result + const bool ret = fs.remove_result(tag); + + // Return file handle if the remove failed + // If remove succeeds then the file id is removed automaticaly. + if (!ret) { + fs.free_file_id(fid); + } + + return ret ? 0 : -1; +} + +int AP_Filesystem_9P2000::mkdir(const char *pathname) +{ + if (hal.scheduler->in_main_thread()) { + // Too slow for the main thread + errno = MAIN_THREAD_ERROR; + return -1; + } + + // Make sure filesystem is mounted. + NineP2000& fs = AP::network().get_filesystem(); + if (!fs.mounted()) { + errno = ENODEV; + return -1; + } + + // Make folder + if (!create_file(fs, pathname, true)) { + errno = EROFS; + return -1; + } + + return 0; +} + +void *AP_Filesystem_9P2000::opendir(const char *pathname) +{ + if (hal.scheduler->in_main_thread()) { + // Too slow for the main thread + errno = MAIN_THREAD_ERROR; + return nullptr; + } + + // Make sure filesystem is mounted. + NineP2000& fs = AP::network().get_filesystem(); + if (!fs.mounted()) { + errno = ENODEV; + return nullptr; + } + + // Find free object + uint8_t idx; + for (idx=0; idxin_main_thread()) { + // Too slow for the main thread + // This really should not happen, because open dir should fail, could panic here + closedir(dirp); + errno = MAIN_THREAD_ERROR; + return nullptr; + } + + uint32_t idx = ((rdir*)dirp) - &dir[0]; + if (idx >= max_open_dir) { + // Invalid rdir obj + errno = EBADF; + return nullptr; + } + + // Make sure filesystem is still mounted. + NineP2000& fs = AP::network().get_filesystem(); + if (!fs.mounted()) { + errno = ENODEV; + return nullptr; + } + + // Read the next item, use large count which will be shortened + // Because the file stat includes names we can't be sure how long it is. + const uint16_t tag = fs.request_dir_read(dir[idx].fileId, dir[idx].ofs, &dir[idx].de); + + // Wait for the reply + if (!wait_for_tag(fs, tag)) { + return nullptr; + } + + // Grab reply, should be none zero + const int32_t size = fs.read_result(tag, true); + if (size <= 0) { + // Got to the last item (or read failed) + return nullptr; + } + + // Increment offset for next call + dir[idx].ofs += size; + + return &dir[idx].de; +} + +int AP_Filesystem_9P2000::closedir(void *dirp) +{ + uint32_t idx = ((rdir *)dirp) - &dir[0]; + if (idx >= max_open_dir) { + errno = EBADF; + return -1; + } + + if ((dir[idx].path != nullptr) && (dir[idx].fileId != 0)) { + AP::network().get_filesystem().free_file_id(dir[idx].fileId); + } + + free(dir[idx].path); + dir[idx].path = nullptr; + return 0; +} + +int AP_Filesystem_9P2000::rename(const char *oldpath, const char *newpath) +{ + if (hal.scheduler->in_main_thread()) { + // Too slow for the main thread + errno = MAIN_THREAD_ERROR; + return -1; + } + + // Make sure filesystem is mounted. + NineP2000& fs = AP::network().get_filesystem(); + if (!fs.mounted()) { + errno = ENODEV; + return -1; + } + + // Should have a common path. + const uint16_t oldlen = strlen(oldpath); + const uint16_t newlen = strlen(newpath); + + uint16_t common = 0; + uint16_t split = 0; + bool found_split = false; + for (uint16_t i = 0; i < MIN(oldlen, newlen); i++) { + if (oldpath[i] == newpath[i]) { + common = i; + } + if ((oldpath[i] == '/') || (newpath[i] == '/')) { + found_split = true; + split = i; + } + } + + // Path must be common, only name different + if (found_split && (common < split)) { + errno = EPERM; + return -1; + } + + // Navigate to the given path + const uint32_t fid = get_file_id(fs, oldpath, NineP2000::walkType::Any); + if (fid == 0) { + errno = ENOENT; + return -1; + } + + // Request rename, pass new name + const uint8_t name_start = split + (found_split ? 1 : 0); + const uint16_t tag = fs.request_rename(fid, &newpath[name_start]); + if (tag == fs.NOTAG) { + return -1; + } + + // Wait for the reply + if (!wait_for_tag(fs, tag)) { + fs.free_file_id(fid); + return -1; + } + + fs.free_file_id(fid); + + return fs.stat_update_result(tag) ? 0 : -1; +} + +// set modification time on a file +bool AP_Filesystem_9P2000::set_mtime(const char *filename, const uint32_t mtime_sec) +{ + if (hal.scheduler->in_main_thread()) { + // Too slow for the main thread + errno = MAIN_THREAD_ERROR; + return false; + } + + // Make sure filesystem is mounted. + NineP2000& fs = AP::network().get_filesystem(); + if (!fs.mounted()) { + errno = ENODEV; + return false; + } + + // Navigate to the given path + const uint32_t fid = get_file_id(fs, filename, NineP2000::walkType::Any); + if (fid == 0) { + errno = ENOENT; + return false; + } + + // Request update, pass new time + const uint16_t tag = fs.request_set_mtime(fid, mtime_sec); + if (tag == fs.NOTAG) { + return -1; + } + + // Wait for the reply + if (!wait_for_tag(fs, tag)) { + fs.free_file_id(fid); + return -1; + } + + fs.free_file_id(fid); + + return fs.stat_update_result(tag); +} + +#endif // AP_NETWORKING_FILESYSTEM_ENABLED diff --git a/libraries/AP_Filesystem/AP_Filesystem_9P2000.h b/libraries/AP_Filesystem/AP_Filesystem_9P2000.h new file mode 100644 index 00000000000000..b1b2c2337cc8b7 --- /dev/null +++ b/libraries/AP_Filesystem/AP_Filesystem_9P2000.h @@ -0,0 +1,84 @@ +/* + 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 + +#if AP_NETWORKING_FILESYSTEM_ENABLED + +#include "AP_Filesystem_backend.h" +#include + +class AP_Filesystem_9P2000 : public AP_Filesystem_Backend +{ +public: + + // functions that closely match the equivalent posix calls + int open(const char *fname, int flags, bool allow_absolute_paths = false) override; + int close(int fd) override; + int32_t read(int fd, void *buf, uint32_t count) override; + int32_t write(int fd, const void *buf, uint32_t count) override; + // int fsync(int fd) override; not supported + int32_t lseek(int fd, int32_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; + int rename(const char *oldpath, const char *newpath) override; + + // No way to get disk details in 9P2000 + // return free disk space in bytes, -1 on error + //int64_t disk_free(const char *path) override; + //int64_t disk_space(const char *path) override; + + // set modification time on a file + bool set_mtime(const char *filename, const uint32_t mtime_sec) override; + +private: + + // Open a given file ID with flags + bool open_fileId(NineP2000& fs, const uint32_t fileId, int flags); + + // Wait for response, blocking + bool wait_for_tag(NineP2000& fs, const uint16_t tag) const; + + // Get the file id for a given name + uint32_t get_file_id(NineP2000& fs, const char *name, const NineP2000::walkType type) const; + + // Create a file or directory with the given name + bool create_file(NineP2000& fs, const char *fname, bool is_dir); + + // 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 { + uint32_t fileId; + uint32_t size; + uint32_t ofs; + } file[max_open_file]; + + // allow up to 4 directory opens + struct rdir { + char *path; + uint32_t fileId; + uint32_t ofs; + struct dirent de; + } dir[max_open_dir]; +}; + +#endif // AP_NETWORKING_FILESYSTEM_ENABLED diff --git a/libraries/AP_Networking/AP_Networking.cpp b/libraries/AP_Networking/AP_Networking.cpp index 12b0fb1ae3f55b..0d8264eaf08a81 100644 --- a/libraries/AP_Networking/AP_Networking.cpp +++ b/libraries/AP_Networking/AP_Networking.cpp @@ -217,6 +217,10 @@ void AP_Networking::init() // init network mapped serialmanager ports ports_init(); #endif + +#if AP_NETWORKING_FILESYSTEM_ENABLED + NineP2000_client.init(); +#endif } /* diff --git a/libraries/AP_Networking/AP_Networking.h b/libraries/AP_Networking/AP_Networking.h index 5c7e8ebca18d62..8e7fc8d9bb6da5 100644 --- a/libraries/AP_Networking/AP_Networking.h +++ b/libraries/AP_Networking/AP_Networking.h @@ -9,6 +9,7 @@ #include "AP_Networking_address.h" #include "AP_Networking_Backend.h" #include "AP_Networking_CAN.h" +#include "AP_Networking_NineP2000.h" #include #include @@ -165,6 +166,11 @@ class AP_Networking return (param.options.get() & int32_t(option)) != 0; } +#if AP_NETWORKING_FILESYSTEM_ENABLED + // Get the 9P2000 client + NineP2000& get_filesystem() { return NineP2000_client; } +#endif + private: static AP_Networking *singleton; @@ -280,6 +286,10 @@ class AP_Networking }; #endif // AP_NETWORKING_REGISTER_PORT_ENABLED +#if AP_NETWORKING_FILESYSTEM_ENABLED + NineP2000 NineP2000_client; +#endif + private: uint32_t announce_ms; diff --git a/libraries/AP_Networking/AP_Networking_Config.h b/libraries/AP_Networking/AP_Networking_Config.h index 342257972f8d7e..1c1291f34587a5 100644 --- a/libraries/AP_Networking/AP_Networking_Config.h +++ b/libraries/AP_Networking/AP_Networking_Config.h @@ -141,3 +141,11 @@ #ifndef AP_NETWORKING_REGISTER_PORT_ENABLED #define AP_NETWORKING_REGISTER_PORT_ENABLED AP_NETWORKING_ENABLED && AP_SERIALMANAGER_REGISTER_ENABLED #endif + +#ifndef AP_NETWORKING_FILESYSTEM_ENABLED +#define AP_NETWORKING_FILESYSTEM_ENABLED (AP_NETWORKING_ENABLED && ((CONFIG_HAL_BOARD == HAL_BOARD_LINUX) || (CONFIG_HAL_BOARD == HAL_BOARD_SITL))) +#endif + +#if AP_NETWORKING_FILESYSTEM_ENABLED && !AP_NETWORKING_ENABLED + #error AP_NETWORKING_FILESYSTEM_ENABLED requires AP_NETWORKING_ENABLED +#endif diff --git a/libraries/AP_Networking/AP_Networking_NineP2000.cpp b/libraries/AP_Networking/AP_Networking_NineP2000.cpp new file mode 100644 index 00000000000000..21f226d8df7f4e --- /dev/null +++ b/libraries/AP_Networking/AP_Networking_NineP2000.cpp @@ -0,0 +1,1488 @@ +#include "AP_Networking_Config.h" + +#if AP_NETWORKING_FILESYSTEM_ENABLED + +#include "AP_Networking.h" +#include +#include +#include +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +const AP_Param::GroupInfo NineP2000::var_info[] = { + // @Param: ENABLE + // @DisplayName: Enable 9P2000 client + // @Description: 9P2000 client allows file access to enteral server over a TCP connection. + // @Values: 0:Disabled, 1:Enabled + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO_FLAGS("ENABLE", 1, NineP2000, enabled, 0, AP_PARAM_FLAG_ENABLE), + + // @Group: IP + // @Path: AP_Networking_address.cpp + // @RebootRequired: True + AP_SUBGROUPINFO(ip, "IP", 2, NineP2000, AP_Networking_IPV4), + + // @Param: PORT + // @DisplayName: Port number + // @Description: Port number + // @Range: 0 65535 + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("PORT", 3, NineP2000, port, 0), + + AP_GROUPEND +}; + +/* + initialise mapped network ports + */ +void NineP2000::init() +{ + sock = NEW_NOTHROW SocketAPM(false); + + const uint32_t bufferSize = 32768; + writebuffer = NEW_NOTHROW ByteBuffer(bufferSize); + if ((sock != nullptr) && (writebuffer != nullptr)) { + sock->set_blocking(true); + writebuffer->set_size_best(bufferSize); + + // Need a reasonable size buffer before starting the thread + if ((writebuffer->get_size() > 256) && + hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&NineP2000::loop, void), "9P2000", 1024, AP_HAL::Scheduler::PRIORITY_STORAGE, 0)) { + return; + } + } + AP_BoardConfig::allocation_error("9P2000"); + +} + +void NineP2000::loop() +{ + AP::network().startup_wait(); + + bool active = false; + while (true) { + if (!active) { + hal.scheduler->delay_microseconds(100); + } + if (sock == nullptr) { + sock = NEW_NOTHROW SocketAPM(false); + if (sock == nullptr) { + continue; + } + sock->set_blocking(true); + connected = false; + } + if (!connected) { + const char *dest = ip.get_str(); + connected = sock->connect(dest, port.get()); + if (!connected) { + delete sock; + sock = nullptr; + // don't try and connect too fast + hal.scheduler->delay(100); + continue; + } + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "9P2000: connected to %s:%u", dest, unsigned(port.get())); + sock->set_blocking(false); + + // Clear message and file tracking + memset(&request, 0, sizeof(request)); + memset(&fileIds, 0, sizeof(fileIds)); + + // Reset write buffer + writebuffer->clear(); + + // Restart connection process + request_version(); + + } + active = update_send() | update_receive(); + } +} + +// Return true if connected and mounted +bool NineP2000::mounted() +{ + return connected && (state == State::Mounted); +} + +// Write from buffer into socket +bool NineP2000::update_send() +{ + WITH_SEMAPHORE(request_sem); + + if (writebuffer->available() == 0) { + return false; + } + + uint32_t write = writebuffer->peekbytes(writebuffer_readbuffer, sizeof(writebuffer_readbuffer)); + if (write == 0) { + return false; + } + + ssize_t written = sock->send(writebuffer_readbuffer, write); + if ((written <= 0) && (errno == ENOTCONN)) { + // Send failed, disconnect + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "9P2000: closed connection"); + delete sock; + sock = nullptr; + return false; + } + + writebuffer->advance(written); + + return writebuffer->available() > 0; +} + +// Deal with incoming data +bool NineP2000::update_receive() +{ + // Use semaphore for thread safety + WITH_SEMAPHORE(request_sem); + + // Send may have resulted in a disconnect + if (sock == nullptr) { + return false; + } + + const auto len = sock->recv(buffer.buffer, sizeof(buffer.buffer), 0); + if (len == 0) { + // Zero means disconnected + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "9P2000: closed connection"); + delete sock; + sock = nullptr; + return false; + } + if (len < 0) { + // Could return -1 if there is no pending data + return false; + } + + parse(len); + return true; +} + +void NineP2000::parse(const uint32_t len) +{ + // Need at least the length the of the massage + if (len < buffer.header.length) { + return; + } + + // Ignore any messages longer than the negotiated max length + if (buffer.header.length > bufferLen) { + return; + } + + // Deal with each message type + const Type msg_type = (Type)buffer.header.type; + switch (msg_type) { + case Type::Rversion: { + if (state != State::Version) { + // Should only get a version response if we asked for one + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } + handle_version(); + break; + } + + case Type::Rattach: { + if (state != State::Attach) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } + handle_attach(); + break; + } + + // Auth and flush are not supported + case Type::Rauth: + case Type::Rflush: + break; + + // Clear tag from clunk + case Type::Rclunk: { + // Note that there is no timeout, so we could leak a tag and a file id + // Clear the tag and file ID so they can be used again + const uint16_t tag = buffer.header.tag; + + // Check tag is valid + if ((tag >= ARRAY_SIZE(request)) || !request[tag].pending) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return; + } + + // Check that the type matches what is expected + if (request[tag].expectedType != Type::Rclunk) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return; + } + + clear_file_id(request[tag].clunk.fileId); + clear_tag(tag); + break; + } + + // Stash result for callback + case Type::Rerror: + case Type::Rwalk: + case Type::Ropen: + case Type::Rcreate: + case Type::Rread: + case Type::Rwrite: + case Type::Rremove: + case Type::Rstat: + case Type::Rwstat: { + // Should be mounted before responses start turning up + if (state != State::Mounted) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return; + } + + // Check tag is valid + const uint16_t tag = buffer.header.tag; + if ((tag >= ARRAY_SIZE(request)) || !request[tag].pending) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return; + } + + // Check that the type matches what is expected (unexpected errors are allowed) + if ((msg_type != Type::Rerror) && (request[tag].expectedType != msg_type)) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return; + } + + switch (msg_type) { + case Type::Rerror: + handle_error(request[tag]); + break; + + case Type::Rwalk: + handle_rwalk(request[tag]); + break; + + case Type::Ropen: + // If we got a valid response the open worked + request[tag].open.result = buffer.header.length == (sizeof(Message::header) + sizeof(Message::Ropen)); + break; + + case Type::Rcreate: + request[tag].create.result = buffer.header.length == (sizeof(Message::header) + sizeof(Message::Rcreate)); + break; + + case Type::Rread: { + if (request[tag].read.is_dir) { + handle_dir_Rread(request[tag]); + } else { + handle_file_Rread(request[tag]); + } + break; + } + + case Type::Rwrite: { + if (buffer.header.length == (sizeof(Message::header) + sizeof(Message::Rwrite))) { + request[tag].write.count = buffer.Rwrite.count; + } + break; + } + + case Type::Rremove: + request[tag].remove.result = buffer.header.length == sizeof(Message::header); + break; + + case Type::Rstat: + handle_Rstat(request[tag]); + break; + + case Type::Rwstat: + request[tag].rwstat.result = buffer.header.length == sizeof(Message::header); + break; + + default: + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + break; + } + + // No longer pending + request[tag].pending = false; + break; + } + + // Not expecting to receive any requests + case Type::Tversion: + case Type::Tauth: + case Type::Tattach: + case Type::Tflush: + case Type::Twalk: + case Type::Topen: + case Type::Tcreate: + case Type::Tread: + case Type::Twrite: + case Type::Tclunk: + case Type::Tremove: + case Type::Tstat: + case Type::Twstat: + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + break; + } + + // Parsed full length of message + if (buffer.header.length >= len) { + return; + } + + // Try parsing the remainder + const uint16_t remaining = len - buffer.header.length; + memmove(&buffer.buffer[0], &buffer.buffer[buffer.header.length], remaining); + + parse(remaining); +} + +// Add a string to the end of a message +bool NineP2000::add_string(Message &msg, const char *str) const +{ + const size_t offset = msg.header.length; + + const uint16_t len = strlen(str); + const uint32_t space = MIN(bufferLen, writebuffer->space()); + if ((offset + sizeof(len) + len) > space) { + // This would be a huge file name! + return false; + } + + // Add string length and string content; + memcpy(&msg.buffer[offset], &len, sizeof(len)); + strncpy_noterm((char*)&msg.buffer[offset + sizeof(len)], str, len); + + msg.header.length += sizeof(len) + len; + + return true; +} + +// Request version and message size +void NineP2000::request_version() +{ + state = State::Version; + bufferLen = 32; // Assume a minimum message length + + buffer.header.type = (uint8_t)Type::Tversion; + buffer.header.tag = NOTAG; + buffer.header.length = sizeof(Message::header) + sizeof(Message::Tversion); + buffer.Tversion.msize = sizeof(Message); + + if (!add_string(buffer, "9P2000")) { + // This should never fail, bufferLen is 32, there is room for the string + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return; + } + + writebuffer->write(buffer.buffer, buffer.header.length); +} + +// handle version response +void NineP2000::handle_version() +{ + // Should be at least the min length, string increases total length + const uint32_t version_len = sizeof(Message::header) + sizeof(Message::Rversion); + if (buffer.header.length < version_len) { + return; + } + + if (buffer.header.tag != NOTAG) { + // tag should always be NOTAG, throw a error? + return; + } + + // Message length should be equal to or less than the value requested + if (buffer.Rversion.msize > sizeof(Message)) { + return; + } + + // Make sure size is sufficient to fit all fixed length messages + // Max len should be 55, with header of 7 we need at least 62 bytes + size_t max_len = sizeof(Message::Tversion) + sizeof(uint16_t); + max_len = MAX(max_len, sizeof(Message::Rversion)); + max_len = MAX(max_len, sizeof(Message::Tattach) + 2 * sizeof(uint16_t)); + max_len = MAX(max_len, sizeof(Message::Rattach)); + max_len = MAX(max_len, sizeof(Message::Tclunk)); + max_len = MAX(max_len, sizeof(Message::Rerror)); + max_len = MAX(max_len, sizeof(Message::Topen)); + max_len = MAX(max_len, sizeof(Message::Ropen)); + max_len = MAX(max_len, sizeof(Message::Tcreate) + sizeof(uint16_t) + sizeof(uint32_t) + sizeof(uint8_t)); + max_len = MAX(max_len, sizeof(Message::Rcreate)); + max_len = MAX(max_len, sizeof(Message::Tread)); + max_len = MAX(max_len, sizeof(Message::Rread)); + max_len = MAX(max_len, sizeof(Message::Twrite)); + max_len = MAX(max_len, sizeof(Message::Rwrite)); + max_len = MAX(max_len, sizeof(Message::Tstat)); + max_len = MAX(max_len, sizeof(Message::Rstat) + 4 * sizeof(uint16_t)); + max_len = MAX(max_len, sizeof(Message::Twstat) + 4 * sizeof(uint16_t)); + max_len = MAX(max_len, sizeof(Message::Twalk) + sizeof(uint16_t)); + max_len = MAX(max_len, sizeof(Message::Rwalk) + sizeof(qid_t)); + + if (buffer.Rversion.msize < (sizeof(Message::header) + max_len)) { + return; + } + + // Get and check string length + const size_t expected_string_len = buffer.header.length - version_len; + if (buffer.Rversion.version_string_len != expected_string_len) { + return; + } + + // String should match what was requested + if (strncmp("9P2000", (char*)&buffer.buffer[version_len], expected_string_len) != 0) { + return; + } + + // Limit to the agreed message length + bufferLen = buffer.Rversion.msize; + + // Try and attach + request_attach(); +} + +// Request attach +void NineP2000::request_attach() +{ + state = State::Attach; + + buffer.header.type = (uint8_t)Type::Tattach; + buffer.header.tag = ARRAY_SIZE(request); // Use a tag that will not be used in normal operation + buffer.header.length = sizeof(Message::header) + sizeof(Message::Tattach); + + // Use zero file id, no auth + buffer.Tattach.fid = 0; + buffer.Tattach.afid = 0; + + // User name ArduPilot, no aname. + if (!add_string(buffer, "ArduPilot") || !add_string(buffer, "")) { + // Negotiated a message length too small for this message!? + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return; + } + + writebuffer->write(buffer.buffer, buffer.header.length); +} + +// Handle attach response +void NineP2000::handle_attach() +{ + // Fixed length message, header and qid + if (buffer.header.length != (sizeof(Message::header) + sizeof(Message::Rattach))) { + return; + } + + // tag should match the request + if (buffer.header.tag != ARRAY_SIZE(request)) { + return; + } + + // Expecting a directory + if (buffer.Rattach.qid.type != qidType::QTDIR) { + return; + } + + state = State::Mounted; + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "9P2000: mounted file system"); +} + +// Return the next available tag, NOTAG is none free +uint16_t NineP2000::get_free_tag() +{ + WITH_SEMAPHORE(request_sem); + + // Must be mounted for operations to be valid + if (state != State::Mounted) { + return NOTAG; + } + + for (uint8_t i = 0; i < ARRAY_SIZE(request); i++) { + if (!request[i].active) { + request[i].active = true; + return i; + } + } + + return NOTAG; +} + +// Return true if there is a response for the given tag +bool NineP2000::tag_response(const uint16_t tag) +{ + if (tag >= ARRAY_SIZE(request)) { + return false; + } + + WITH_SEMAPHORE(request_sem); + return request[tag].active && !request[tag].pending; +} + +// Return true if there is a response for the given tag with type +bool NineP2000::tag_response_type(const uint16_t tag, const Type type) +{ + WITH_SEMAPHORE(request_sem); + return tag_response(tag) && (request[tag].expectedType == type); +} + +// Called when a command is timed out +void NineP2000::clear_tag(const uint16_t tag) +{ + if (tag >= ARRAY_SIZE(request)) { + return; + } + + WITH_SEMAPHORE(request_sem); + memset(&request[tag], 0, sizeof(request[tag])); +} + +// Generate a new unique file id +uint32_t NineP2000::generate_unique_file_id() +{ + // Use array index as file ID, increment by 1 to keep 0 as special case for root. + WITH_SEMAPHORE(request_sem); + + for (uint8_t i = 0; i < ARRAY_SIZE(fileIds); i++) { + if (!fileIds[i].active) { + fileIds[i].active = true; + fileIds[i].clunked = false; + return i + 1; + } + } + return 0; +} + +// Clear a file id now the file has been closed +void NineP2000::clear_file_id(const uint32_t fileId) +{ + WITH_SEMAPHORE(request_sem); + + const uint32_t index = fileId - 1; + + // Index should be valid and ID should active + if ((index >= ARRAY_SIZE(fileIds)) || !fileIds[index].active) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return; + } + + fileIds[index].active = false; +} + +// Check if a given ID active +bool NineP2000::valid_file_id(const uint32_t fileId) +{ + const uint32_t index = fileId - 1; + return (index < ARRAY_SIZE(fileIds)) && fileIds[index].active && !fileIds[index].clunked; +} + +// Walk to a new file or directory, return tag, NOTAG if failed +uint16_t NineP2000::request_walk(const char* path, const walkType type) +{ + WITH_SEMAPHORE(request_sem); + + // Check there is room for the constant size part of the message + const uint32_t length = sizeof(Message::header) + sizeof(Message::Twalk); + if (writebuffer->space() < length) { + return NOTAG; + } + + // See if there are any tags free + const uint16_t tag = get_free_tag(); + if (tag == NOTAG) { + return NOTAG; + } + + // Get a new file ID + const uint32_t id = generate_unique_file_id(); + + // Check if id is valid + if (id == 0) { + clear_tag(tag); + return NOTAG; + } + + // Fill in message + buffer.header.type = (uint8_t)Type::Twalk; + buffer.header.tag = tag; + buffer.header.length = length; + + // Start at root + buffer.Twalk.fid = 0; + + // End at new id + buffer.Twalk.newfid = id; + + // Zero steps to start with + buffer.Twalk.nwname = 0; + + // Step through path and add strings + const uint16_t len = strlen(path); + uint16_t name_start = 0; + for (uint16_t i = 0; i < len; i++) { + const bool split = path[i] == '/'; + const bool last = i == (len - 1); + if (split || last) { + // Should check total length of message is still valid + const uint16_t name_len = i - name_start + (split ? 0 : 1); + const uint32_t new_len = buffer.header.length + sizeof(name_len) + name_len; + + // Check total message length is still valid + const uint32_t space = MIN(bufferLen, writebuffer->space()); + if (new_len > space) { + clear_tag(tag); + return NOTAG; + } + + // Add string length + memcpy(&buffer.buffer[buffer.header.length], &name_len, sizeof(name_len)); + buffer.header.length += sizeof(name_len); + + // Add string itself + memcpy(&buffer.buffer[buffer.header.length], &path[name_start], name_len); + buffer.header.length += name_len; + + // Increment number of names + buffer.Twalk.nwname += 1; + + // Next section starts after this one + name_start = i + 1; + } + } + + // Make tag as active + request[tag].pending = true; + request[tag].expectedType = Type::Rwalk; + request[tag].walk.fileId = id; + request[tag].walk.type = type; + + // Send command + writebuffer->write(buffer.buffer, buffer.header.length); + + return tag; +} + +void NineP2000::handle_error(Request& result) +{ + // null terminate string and print directly out of buffer + const uint16_t len = buffer.Rerror.ename_string_len; + const uint32_t string_start = sizeof(Message::header) + sizeof(Message::Rerror); + buffer.buffer[string_start + MIN(len, 100)] = 0; + + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "9P2000: error: %s", (char*)&buffer.buffer[string_start]); + + // Set the appropiate fail response for the expected message + switch (result.expectedType) { + case Type::Rwalk: + result.walk.fileId = 0; + break; + + case Type::Ropen: + result.open.result = false; + break; + + case Type::Rcreate: + result.create.result = false; + break; + + case Type::Rread: + result.read.count = -1; + break; + + case Type::Rwrite: + result.write.count = -1; + break; + + case Type::Rremove: + result.remove.result = false; + break; + + case Type::Rstat: + result.stat.result = false; + break; + + case Type::Rwstat: + result.rwstat.result = false; + break; + + default: + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + break; + } +} + +void NineP2000::handle_rwalk(Request& result) +{ + WITH_SEMAPHORE(request_sem); + + const uint16_t num_ids = buffer.Rwalk.nwqid; + + // Zero length walk means we must be root + if (num_ids == 0) { + // Root must be a dir + if (result.walk.type != walkType::Directory) { + free_file_id(result.walk.fileId); + result.walk.fileId = 0; + return; + } + } + + // Calculate the offset of the last id + const uint16_t id_offset = sizeof(Message::header) + sizeof(Message::Rwalk) + (num_ids - 1) * sizeof(qid_t); + + // Make sure the message is long enough + if (buffer.header.length != (id_offset + sizeof(qid_t))) { + free_file_id(result.walk.fileId); + result.walk.fileId = 0; + return; + } + + // Expecting the correct type + if (result.walk.type != walkType::Any) { + + // Read in last id + const qid_t &qid = (qid_t&)buffer.buffer[id_offset]; + + const uint8_t expected_type = (result.walk.type == walkType::Directory) ? qidType::QTDIR : qidType::QTFILE; + if (qid.type != expected_type) { + free_file_id(result.walk.fileId); + result.walk.fileId = 0; + return; + } + } + + // Got this far then fileId is valid, wait for the caller to pickup the result +} + +uint32_t NineP2000::walk_result(const uint16_t tag) +{ + WITH_SEMAPHORE(request_sem); + + // Make sure the tag is valid and there is a waiting response + if (!tag_response_type(tag, Type::Rwalk)) { + clear_tag(tag); + return 0; + } + + // Get file id + const uint32_t fileId = request[tag].walk.fileId; + + // success, return id + clear_tag(tag); + return fileId; +} + +// Return the file id to the server for re-use +void NineP2000::free_file_id(const uint32_t id) +{ + WITH_SEMAPHORE(request_sem); + + // Check id is valid + if (!valid_file_id(id)) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + } + + // Check there is room for the message + const uint32_t length = sizeof(Message::header) + sizeof(Message::Tclunk); + if (length > writebuffer->space()) { + return; + } + + // See if there are any tags free + const uint16_t tag = get_free_tag(); + if (tag == NOTAG) { + // This is bad, it means we leak a file id. + return; + } + + // Mark as clunked so we only free once + fileIds[id - 1].clunked = true; + + // Mark tag as active + request[tag].pending = true; + request[tag].expectedType = Type::Rclunk; + request[tag].clunk.fileId = id; + + // Fill in message + buffer.header.type = (uint8_t)Type::Tclunk; + buffer.header.tag = tag; + buffer.header.length = sizeof(Message::header) + sizeof(Message::Tclunk); + buffer.Tclunk.fid = id; + + writebuffer->write(buffer.buffer, buffer.header.length); +} + +// Request read of given file or directory with given flags +uint16_t NineP2000::request_open(const uint32_t id, const int flags) +{ + WITH_SEMAPHORE(request_sem); + + // Check there is room for the message + const uint32_t length = sizeof(Message::header) + sizeof(Message::Topen); + if (length > writebuffer->space()) { + return NOTAG; + } + + // See if there are any tags free + const uint16_t tag = get_free_tag(); + if (tag == NOTAG) { + return NOTAG; + } + + // ID invalid + if (!valid_file_id(id)) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return NOTAG; + } + + // Mark tag as active + request[tag].pending = true; + request[tag].expectedType = Type::Ropen; + + // Translate flags to mode + uint8_t mode = 0; + if ((flags & O_RDWR) != 0) { + mode = openMode::ORDWR; + + } else if ((flags & O_WRONLY) != 0) { + mode = openMode::OWRITE; + + } else { + mode = openMode::OREAD; + } + + // Fill in message + buffer.header.type = (uint8_t)Type::Topen; + buffer.header.tag = tag; + buffer.header.length = length; + buffer.Topen.fid = id; + buffer.Topen.mode = mode; + + writebuffer->write(buffer.buffer, buffer.header.length); + + return tag; +} + +// Get open result, returns true if successful +bool NineP2000::open_result(const uint16_t tag) +{ + WITH_SEMAPHORE(request_sem); + + // Make sure the tag is valid and there is a waiting response + if (!tag_response_type(tag, Type::Ropen)) { + clear_tag(tag); + return false; + } + + const bool ret = request[tag].open.result; + + clear_tag(tag); + return ret; +} + +// Return the maximum length that can be read in a single packet +// This is only valid if the file system is mounted as bufferlength is negotiated +uint32_t NineP2000::max_read_len() const { + const uint16_t data_offset = sizeof(Message::header) + sizeof(Message::Rread); + return bufferLen - data_offset; +} + +// Read a directory, return tag, NOTAG if failed +uint16_t NineP2000::request_dir_read(const uint32_t id, const uint64_t offset, struct dirent *de) +{ + WITH_SEMAPHORE(request_sem); + + // ID invalid + if (!valid_file_id(id)) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return NOTAG; + } + + // Check there is room for the message + const uint32_t length = sizeof(Message::header) + sizeof(Message::Tread); + if (length > writebuffer->space()) { + return NOTAG; + } + + // See if there are any tags free + const uint16_t tag = get_free_tag(); + if (tag == NOTAG) { + return NOTAG; + } + + // Mark tag as active + request[tag].pending = true; + request[tag].expectedType = Type::Rread; + request[tag].read.is_dir = true; + request[tag].read.dir = de; + + // Fill in message + buffer.header.type = (uint8_t)Type::Tread; + buffer.header.tag = tag; + buffer.header.length = length; + buffer.Tread.fid = id; + buffer.Tread.offset = offset; + + // We don't know how long the directory entry will be as it has variable length strings + // Just read max length for now + buffer.Tread.count = max_read_len(); + + writebuffer->write(buffer.buffer, buffer.header.length); + + return tag; +} + +void NineP2000::handle_dir_Rread(Request& result) +{ + WITH_SEMAPHORE(request_sem); + + // Need a place to put the result + if (result.read.dir == nullptr) { + return; + } + + // Should at least contain header count and stat + const uint16_t stat_end = sizeof(Message::header) + sizeof(Message::Rread) + sizeof(stat_t); + if (buffer.header.length < stat_end) { + return; + } + + const stat_t &info = (stat_t&)buffer.buffer[sizeof(Message::header) + sizeof(Message::Rread)]; + + // Length feild does not include itself + const uint16_t stat_len = info.msg_size + sizeof(stat_t::msg_size); + + // Make sure there is room for the whole stat now we know the full size + if (buffer.header.length < (sizeof(Message::Rread) + stat_len)) { + return; + } + + // Check file mode + if ((info.qid.type != qidType::QTFILE) && (info.qid.type != qidType::QTDIR)) { + return; + } + + // All checks done, now we can update the directory entry + + // Copy name, comes after the stat + uint16_t name_len = 0; + memcpy(&name_len, &buffer.buffer[stat_end], sizeof(name_len)); + + memset(result.read.dir->d_name, 0, sizeof(result.read.dir->d_name)); + strncpy(result.read.dir->d_name, (char*)&buffer.buffer[stat_end + sizeof(name_len)], MIN(sizeof(result.read.dir->d_name), name_len)); + + // Fill in file flags + if (info.qid.type == qidType::QTFILE) { + result.read.dir->d_type = DT_REG; + } else{ + result.read.dir->d_type = DT_DIR; + } + + result.read.count = stat_len; +} + +// Read a file, return tag, NOTAG if failed +uint16_t NineP2000::request_file_read(const uint32_t id, const uint64_t offset, const uint32_t count, void *buf) +{ + WITH_SEMAPHORE(request_sem); + + // ID invalid + if (!valid_file_id(id)) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return NOTAG; + } + + // Check there is room for the message + const uint32_t length = sizeof(Message::header) + sizeof(Message::Tread); + if (length > writebuffer->space()) { + return NOTAG; + } + + // See if there are any tags free + const uint16_t tag = get_free_tag(); + if (tag == NOTAG) { + return NOTAG; + } + + // Mark tag as active + request[tag].pending = true; + request[tag].expectedType = Type::Rread; + request[tag].read.is_dir = false; + request[tag].read.buf = buf; + request[tag].read.count = count; + + // Fill in message + buffer.header.type = (uint8_t)Type::Tread; + buffer.header.tag = tag; + buffer.header.length = length; + buffer.Tread.fid = id; + buffer.Tread.offset = offset; + buffer.Tread.count = count; + + writebuffer->write(buffer.buffer, buffer.header.length); + + return tag; +} + +void NineP2000::handle_file_Rread(Request& result) +{ + WITH_SEMAPHORE(request_sem); + + // Need a place to put the result + if (result.read.buf == nullptr) { + result.read.count = -1; + return; + } + + // Not expecting to get more data than was asked for + if (buffer.Rread.count > uint32_t(result.read.count)) { + result.read.count = -1; + return; + } + result.read.count = buffer.Rread.count; + + // Copy result + memcpy(result.read.buf, &buffer.buffer[sizeof(Message::header) + sizeof(Message::Rread)], result.read.count); +} + +// Fill in a directory item based on the read result, returns none zero if success +int32_t NineP2000::read_result(const uint16_t tag, const bool is_dir) +{ + WITH_SEMAPHORE(request_sem); + + // Make sure the tag is valid and there is a waiting response + if (!tag_response_type(tag, Type::Rread)) { + clear_tag(tag); + return -1; + } + + // Should be for the expected type + if (request[tag].read.is_dir != is_dir) { + clear_tag(tag); + return -1; + } + + const int32_t count = request[tag].read.count; + + clear_tag(tag); + return count; +} + +// Request stat for a given file id +uint16_t NineP2000::request_stat(const uint32_t id, struct stat *stbuf) +{ + WITH_SEMAPHORE(request_sem); + + // ID invalid + if (!valid_file_id(id)) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return NOTAG; + } + + // Check there is room for the message + const uint32_t length = sizeof(Message::header) + sizeof(Message::Tstat); + if (length > writebuffer->space()) { + return NOTAG; + } + + // See if there are any tags free + const uint16_t tag = get_free_tag(); + if (tag == NOTAG) { + return NOTAG; + } + + // Mark tag as active + request[tag].pending = true; + request[tag].expectedType = Type::Rstat; + request[tag].stat.stbuf = stbuf; + + // Fill in message + buffer.header.type = (uint8_t)Type::Tstat; + buffer.header.tag = tag; + buffer.header.length = length; + buffer.Tstat.fid = id; + + writebuffer->write(buffer.buffer, buffer.header.length); + + return tag; +} + +// Handle a stat response +void NineP2000::handle_Rstat(Request& result) +{ + WITH_SEMAPHORE(request_sem); + + // Clear stats + memset(result.stat.stbuf, 0, sizeof(*result.stat.stbuf)); + + // length in bytes + result.stat.stbuf->st_size = buffer.Rstat.stat.length; + + // Access and modification timestamps + result.stat.stbuf->st_atime = buffer.Rstat.stat.atime; + result.stat.stbuf->st_mtime = buffer.Rstat.stat.mtime; + + // Fill in file flags + const uint8_t &mode = buffer.Rstat.stat.qid.type; + if (mode == qidType::QTFILE) { + result.stat.stbuf->st_mode |= S_IFREG; + } else if (mode == qidType::QTDIR) { + result.stat.stbuf->st_mode |= S_IFDIR; + } + + result.stat.result = true; +} + +// Get stat result, returns true if successful +bool NineP2000::stat_result(const uint16_t tag) +{ + WITH_SEMAPHORE(request_sem); + + // Make sure the tag is valid and there is a waiting response + if (!tag_response_type(tag, Type::Rstat)) { + clear_tag(tag); + return false; + } + + const bool ret = request[tag].stat.result; + + clear_tag(tag); + return ret; +} + +// Return the maximum length that can be written in a single packet +// This is only valid if the file system is mounted as bufferlength is negotiated +uint32_t NineP2000::max_write_len() const { + const uint16_t data_offset = sizeof(Message::header) + sizeof(Message::Twrite); + return bufferLen - data_offset; +} + +// Request write for given file id, return tag +uint16_t NineP2000::request_write(const uint32_t id, const uint64_t offset, uint32_t count, const void *buf) +{ + WITH_SEMAPHORE(request_sem); + + // ID invalid + if (!valid_file_id(id)) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return NOTAG; + } + + // Limit write to max packet size + const uint16_t data_offset = sizeof(Message::header) + sizeof(Message::Twrite); + count = MIN(count, uint32_t(bufferLen - data_offset)); + + // Check there is room for the message + const uint32_t length = data_offset + count; + if (length > writebuffer->space()) { + return NOTAG; + } + + // See if there are any tags free + const uint16_t tag = get_free_tag(); + if (tag == NOTAG) { + return NOTAG; + } + + // Mark tag as active + request[tag].pending = true; + request[tag].expectedType = Type::Rwrite; + + // Fill in message + buffer.header.type = (uint8_t)Type::Twrite; + buffer.header.tag = tag; + buffer.header.length = length; + buffer.Twrite.fid = id; + buffer.Twrite.offset = offset; + buffer.Twrite.count = count; + + memcpy(&buffer.buffer[data_offset], buf, count); + + writebuffer->write(buffer.buffer, buffer.header.length); + + return tag; +} + +int32_t NineP2000::write_result(const uint16_t tag) +{ + WITH_SEMAPHORE(request_sem); + + // Make sure the tag is valid and there is a waiting response + if (!tag_response_type(tag, Type::Rwrite)) { + clear_tag(tag); + return -1; + } + + // Should be a create response + if (request[tag].expectedType != Type::Rwrite) { + clear_tag(tag); + return -1; + } + + const int32_t ret = request[tag].write.count; + + clear_tag(tag); + + return ret; +} + +// Request create for given directory id, return tag +uint16_t NineP2000::request_create(const uint32_t id, const char*name, const bool dir) +{ + WITH_SEMAPHORE(request_sem); + + // ID invalid + if (!valid_file_id(id)) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return NOTAG; + } + + // See if there are any tags free + const uint16_t tag = get_free_tag(); + if (tag == NOTAG) { + return NOTAG; + } + + // Mark tag as active + request[tag].pending = true; + request[tag].expectedType = Type::Rcreate; + + // Fill in message + buffer.header.type = (uint8_t)Type::Tcreate; + buffer.header.tag = tag; + buffer.header.length = sizeof(Message::header) + sizeof(Message::Tcreate); + buffer.Tcreate.fid = id; + + // Give everyone rwx permissions + const uint32_t perm = 0777 | (dir ? (qidType::QTDIR << 24) : 0); + const uint8_t mode = 0; + + // permissions and mode come after the variable length string + const uint8_t tail_len = sizeof(perm) + sizeof(mode); + + if (!add_string(buffer, name) || ((buffer.header.length + tail_len) > MIN(bufferLen, writebuffer->space()))) { + // Ran out of room in the message + clear_tag(tag); + return NOTAG; + } + + // Fill in tail now string length is set + memcpy(&buffer.buffer[buffer.header.length], &perm, sizeof(perm)); + buffer.header.length += sizeof(perm); + memcpy(&buffer.buffer[buffer.header.length], &mode, sizeof(mode)); + buffer.header.length += sizeof(mode); + + writebuffer->write(buffer.buffer, buffer.header.length); + + return tag; +} + +bool NineP2000::create_result(const uint16_t tag) +{ + WITH_SEMAPHORE(request_sem); + + // Make sure the tag is valid and there is a waiting response + if (!tag_response_type(tag, Type::Rcreate)) { + clear_tag(tag); + return false; + } + + const bool ret = request[tag].create.result; + + clear_tag(tag); + + return ret; +} + +// Request remove for given id, return tag +uint16_t NineP2000::request_remove(const uint32_t id) +{ + WITH_SEMAPHORE(request_sem); + + // ID invalid + if (!valid_file_id(id)) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return NOTAG; + } + + // Check there is room for the message + const uint32_t length = sizeof(Message::header) + sizeof(Message::Tremove); + if (length > writebuffer->space()) { + return NOTAG; + } + + // See if there are any tags free + const uint16_t tag = get_free_tag(); + if (tag == NOTAG) { + return NOTAG; + } + + // Mark tag as active + request[tag].pending = true; + request[tag].expectedType = Type::Rremove; + + // Fill in message + buffer.header.type = (uint8_t)Type::Tremove; + buffer.header.tag = tag; + buffer.header.length = length; + + buffer.Tremove.fid = id; + + writebuffer->write(buffer.buffer, buffer.header.length); + + return tag; +} + +// Get result of remove +bool NineP2000::remove_result(const uint16_t tag) +{ + WITH_SEMAPHORE(request_sem); + + // Make sure the tag is valid and there is a waiting response + if (!tag_response_type(tag, Type::Rremove)) { + clear_tag(tag); + return false; + } + + const bool ret = request[tag].remove.result; + + clear_tag(tag); + + return ret; +} + +// Request rename for given id, return tag +uint16_t NineP2000::request_rename(const uint32_t id, const char*name) +{ + WITH_SEMAPHORE(request_sem); + + // ID invalid + if (!valid_file_id(id)) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return NOTAG; + } + + // Check there is room for constant lenght part of the message, assume all four strings are zero length + const uint32_t length = sizeof(Message::header) + sizeof(Message::Twstat); + if ((length + (4 * 2)) > writebuffer->space()) { + return NOTAG; + } + + // See if there are any tags free + const uint16_t tag = get_free_tag(); + if (tag == NOTAG) { + return NOTAG; + } + + // Mark tag as active + request[tag].pending = true; + request[tag].expectedType = Type::Rwstat; + + // Fill in message + buffer.header.type = (uint8_t)Type::Twstat; + buffer.header.tag = tag; + buffer.header.length = length; + + buffer.Twstat.fid = id; + buffer.Twstat.nstat = 1; + + // Max values indicate don't change. + memset(&buffer.Twstat.stat, 0xFF, sizeof(buffer.Twstat.stat)); + + // Add new name + if (!add_string(buffer, name) || + // Don't change other names + !add_string(buffer, "") || + !add_string(buffer, "") || + !add_string(buffer, "")) { + + // Ran out of room in the message + clear_tag(tag); + return NOTAG; + } + + writebuffer->write(buffer.buffer, buffer.header.length); + + return tag; +} + +// Get result of rename and mtime set +bool NineP2000::stat_update_result(const uint16_t tag) +{ + WITH_SEMAPHORE(request_sem); + + // Make sure the tag is valid and there is a waiting response + if (!tag_response_type(tag, Type::Rwstat)) { + clear_tag(tag); + return false; + } + + const bool ret = request[tag].rwstat.result; + + // Finished with tag + clear_tag(tag); + + return ret; +} + +// Request mtime update for given id, return tag +uint16_t NineP2000::request_set_mtime(const uint32_t id, const uint32_t mtime) +{ + WITH_SEMAPHORE(request_sem); + + // ID invalid + if (!valid_file_id(id)) { + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + return NOTAG; + } + + // Check there is room for constant lenght part of the message, assume all four strings are zero length + const uint32_t length = sizeof(Message::header) + sizeof(Message::Twstat); + if ((length + (4 * 2)) > writebuffer->space()) { + return NOTAG; + } + + // See if there are any tags free + const uint16_t tag = get_free_tag(); + if (tag == NOTAG) { + return NOTAG; + } + + // Mark tag as active + request[tag].pending = true; + request[tag].expectedType = Type::Rwstat; + + // Fill in message + buffer.header.type = (uint8_t)Type::Twstat; + buffer.header.tag = tag; + buffer.header.length = length; + + buffer.Twstat.fid = id; + buffer.Twstat.nstat = 1; + + // Max values indicate don't change. + memset(&buffer.Twstat.stat, 0xFF, sizeof(Message::Twstat.stat)); + + // Set mtime + buffer.Twstat.stat.mtime = mtime; + + // Don't change names + if (!add_string(buffer, "") || + !add_string(buffer, "") || + !add_string(buffer, "") || + !add_string(buffer, "")) { + + // Ran out of room in the message + clear_tag(tag); + return NOTAG; + } + + writebuffer->write(buffer.buffer, buffer.header.length); + + return tag; +} + +#endif // AP_NETWORKING_FILESYSTEM_ENABLED diff --git a/libraries/AP_Networking/AP_Networking_NineP2000.h b/libraries/AP_Networking/AP_Networking_NineP2000.h new file mode 100644 index 00000000000000..7e4bc236b8f725 --- /dev/null +++ b/libraries/AP_Networking/AP_Networking_NineP2000.h @@ -0,0 +1,455 @@ +#include "AP_Networking_Config.h" + +#if AP_NETWORKING_FILESYSTEM_ENABLED + +class NineP2000 { +public: + /* Do not allow copies */ + CLASS_NO_COPY(NineP2000); + + NineP2000() {}; + + void init(); + + static const struct AP_Param::GroupInfo var_info[]; + AP_Int8 enabled; + AP_Networking_IPV4 ip {"0.0.0.0"}; + AP_Int32 port; + SocketAPM *sock; + + // Return true if connected and mounted + bool mounted(); + + // Return true if there is a response for the given tag + bool tag_response(const uint16_t tag); + + // Called when a command is timed out + void clear_tag(const uint16_t tag); + + // Destination of walk + enum walkType { + File, + Directory, + Any, + }; + + // Walk to a new file or directory, return tag, NOTAG if failed + uint16_t request_walk(const char* path, const walkType type); + + // Check if the walk result is valid for a directory or file + uint32_t walk_result(const uint16_t tag); + + // Return the file id to the server for re-use + void free_file_id(const uint32_t id); + + // Request read of given file or directory with given flags + uint16_t request_open(const uint32_t id, const int flags); + + // Return true if open success + bool open_result(const uint16_t tag); + + // Request read of a file, return tag, NOTAG if failed + uint16_t request_file_read(const uint32_t id, const uint64_t offset, const uint32_t count, void *buf); + + // Request read of a directory, return tag, NOTAG if failed + uint16_t request_dir_read(const uint32_t id, const uint64_t offset, struct dirent *de); + + // Get result of read request, Return the number of bytes read, -1 for error + int32_t read_result(const uint16_t tag, const bool is_dir); + + // Request stat for a given file id, return tag, NOTAG if failed + uint16_t request_stat(const uint32_t id, struct stat *stbuf); + + // Get stat result + bool stat_result(const uint16_t tag); + + // Request write for given file id, return tag + uint16_t request_write(const uint32_t id, const uint64_t offset, uint32_t count, const void *buf); + + // Return the number of bytes written, -1 for error + int32_t write_result(const uint16_t tag); + + // Request create for given directory id, return tag + uint16_t request_create(const uint32_t id, const char*name, const bool dir); + + // Get create result + bool create_result(const uint16_t tag); + + // Request remove for given id, return tag + uint16_t request_remove(const uint32_t id); + + // Get remove result + bool remove_result(const uint16_t tag); + + // Request rename for given id, return tag + uint16_t request_rename(const uint32_t id, const char*name); + + // Request mtime update for given id, return tag + uint16_t request_set_mtime(const uint32_t id, const uint32_t mtime); + + // Get rename result and mtime result + bool stat_update_result(const uint16_t tag); + + // Magic value for invalid tag + static constexpr uint16_t NOTAG = 0xFFFF; + + // Return the maximum length that can be read in a single packet + // This is only valid if the file system is mounted as bufferlength is negotiated + uint32_t max_read_len() const; + + // Return the maximum length that can be written in a single packet + // This is only valid if the file system is mounted as bufferlength is negotiated + uint32_t max_write_len() const; + +private: + void loop(); + bool connected; + + bool update_send(); + bool update_receive(); + void parse(const uint32_t len); + + // State of connection process + enum class State { + Version, + Attach, + Mounted, + } state; + + // qid structure is used in several messages + struct PACKED qid_t { + uint8_t type; + uint32_t vers; + uint64_t path; + }; + + // Static part of stat structure, followed by four variable length strings + struct PACKED stat_t { + uint16_t msg_size; + uint16_t type; + uint32_t dev; + qid_t qid; + uint32_t mode; + uint32_t atime; + uint32_t mtime; + uint64_t length; + // name string with length + // uid string with length + // gid string with length + // muid string with length + }; + + // Message shape + union PACKED Message { + // This is the TCP maximum segment size (on most systems) + // Limiting the max packet length to it means packets don't need to be re-assembled. + uint8_t buffer[1460*4]; + + struct PACKED { + // Header is used on all messages + struct PACKED { + uint32_t length; + uint8_t type; + uint16_t tag; + } header; + + // Message payloads differ + union PACKED { + + // Version request and response + struct PACKED { + uint32_t msize; + // version string with length + } Tversion; + + struct PACKED { + uint32_t msize; + uint16_t version_string_len; + // version string of length version_string_len + } Rversion; + + // Attach request and response + struct PACKED { + uint32_t fid; + uint32_t afid; + // uname string with length + // aname string with length + } Tattach; + + struct PACKED { + qid_t qid; + } Rattach; + + // Clunk request + struct PACKED { + uint32_t fid; + } Tclunk; + + // Error response + struct PACKED { + uint16_t ename_string_len; + // ename string with length + } Rerror; + + // Open request and response + struct PACKED { + uint32_t fid; + uint8_t mode; + } Topen; + + struct PACKED { + qid_t qid; + uint32_t iounit; + } Ropen; + + // Create request and response + struct PACKED { + uint32_t fid; + // name string with length + // uint32_t perm + // uint8_t mode + } Tcreate; + + struct PACKED { + qid_t qid; + uint32_t iounit; + } Rcreate; + + // Read request and response + struct PACKED { + uint32_t fid; + uint64_t offset; + uint32_t count; + } Tread; + + struct PACKED { + uint32_t count; + // count * data + } Rread; + + // Write request and response + struct PACKED { + uint32_t fid; + uint64_t offset; + uint32_t count; + // count * data + } Twrite; + + struct PACKED { + uint32_t count; + } Rwrite; + + // Remove request + struct PACKED { + uint32_t fid; + } Tremove; + + // Stat request and response + struct PACKED { + uint32_t fid; + } Tstat; + + struct PACKED { + uint16_t stat_len; + stat_t stat; + // note stat has variable length strings! + } Rstat; + + // Stat write request + struct PACKED { + uint32_t fid; + uint16_t nstat; + stat_t stat; + // note stat has variable length strings! + } Twstat; + + // Walk request and response + struct PACKED { + uint32_t fid; + uint32_t newfid; + uint16_t nwname; + // nwname * wname string with length + } Twalk; + + struct PACKED { + uint16_t nwqid; + // nwqid * qid_t + } Rwalk; + + }; + }; + }; + + enum qidType { + QTDIR = (1 << 7), // DMDIR bit 31, directory + QTAPPEND = (1 << 6), // DMAPPEND bit 30, append only + QTEXCL = (1 << 5), // DMEXCL bit 29, exclusive use + // Not used + QTAUTH = (1 << 3), // DMAUTH bit 27, authentication file + QTTMP = (1 << 2), // DMTMP bit 26, temporay + QTFILE = 0, + }; + + enum openMode { + OREAD = 0, // Read only + OWRITE = 1, // Write only + ORDWR = 2, // Read and write + OEXEC = 3, // Execute + NONE = 4, + OTRUNC = 0x10, + ORCLOSE = 0x40, + }; + + enum class Type: uint8_t { + Tversion = 100, + Rversion = 101, + Tauth = 102, + Rauth = 103, + Tattach = 104, + Rattach = 105, + Rerror = 107, + Tflush = 108, + Rflush = 109, + Twalk = 110, + Rwalk = 111, + Topen = 112, + Ropen = 113, + Tcreate = 114, + Rcreate = 115, + Tread = 116, + Rread = 117, + Twrite = 118, + Rwrite = 119, + Tclunk = 120, + Rclunk = 121, + Tremove = 122, + Rremove = 123, + Tstat = 124, + Rstat = 125, + Twstat = 126, + Rwstat = 127, + }; + + // Object for holding responses + // Tag is used as index into array + // Array length is the max number of concurrent operations + struct Request { + bool active; + bool pending; + Type expectedType; + + // Union stores variables useful for each message type + union { + struct { + uint32_t fileId; + walkType type; + } walk; + + struct { + uint32_t fileId; + } clunk; + + struct { + bool result; + } open; + + struct { + bool is_dir; + int32_t count; + union { + struct dirent *dir; + void *buf; + }; + } read; + + struct { + bool result; + struct stat *stbuf; + } stat; + + struct { + bool result; + } remove; + + struct { + bool result; + } rwstat; + + struct { + bool result; + } create; + + struct { + int32_t count; + } write; + + }; + } request[8]; + + // Send/Receive buffer + Message buffer; + + // Buffer length must be negotiated. + uint16_t bufferLen; + + // Add a string to a message + bool add_string(Message &msg, const char *str) const WARN_IF_UNUSED; + + // Request version and message size + void request_version(); + + // Handle version response + void handle_version(); + + // Request attach + void request_attach(); + + // Handle attach response + void handle_attach(); + + // Handle walk response + void handle_rwalk(Request& result); + + // Handle directory read response + void handle_dir_Rread(Request& result); + + // Handle file read response + void handle_file_Rread(Request& result); + + // Handle a stat response + void handle_Rstat(Request& result); + + // Return true if there is a response for the given tag with type + bool tag_response_type(const uint16_t tag, const Type type); + + // Active file IDs, cannot used concurrently + // 0 is always root and means unused + struct { + bool active; + bool clunked; + } fileIds[ARRAY_SIZE(request)]; + + // Generate a new unique file id + uint32_t generate_unique_file_id(); + + // Clear a file id now the file has been closed + void clear_file_id(const uint32_t fileId); + + // Check if a given ID active + bool valid_file_id(const uint32_t fileId); + + // Return the next available tag, NOTAG is none free + uint16_t get_free_tag(); + + // Decode error messaged print + void handle_error(Request& result); + + // Buffer for outgoing data + ByteBuffer *writebuffer; + uint8_t writebuffer_readbuffer[1460]; + + // Semaphore should be take any time the request array is used + HAL_Semaphore request_sem; +}; + +#endif // AP_NETWORKING_FILESYSTEM_ENABLED \ No newline at end of file diff --git a/libraries/AP_Scripting/examples/FileIOBenchMark.lua b/libraries/AP_Scripting/examples/FileIOBenchMark.lua new file mode 100644 index 00000000000000..8ebb1eba9f23a8 --- /dev/null +++ b/libraries/AP_Scripting/examples/FileIOBenchMark.lua @@ -0,0 +1,101 @@ + +local SIZE = 2097152 -- 2^21 +local BLOCKS = { 16, 32, 64, 128, 256, 512, 1024, 2048, 4096, 8192, 16384, 32768, 65536 } + +local name = "test.txt" +--local name = "@9P2000/test.txt" +local tests = 100 + +local function test_write(block_size) + + -- Generate a random block + local data = "" + while #data < block_size do + data = data .. string.char(math.random(255)) + end + assert(#data == block_size) + + local start_us = micros() + + local file = assert(io.open(name, "w+")) + + -- Repeat the block to make up the file size + local i = 0 + while i < SIZE do + file:write(data) + i = i + #data + end + assert(i == SIZE) + + file:close() + + return (micros() - start_us):tofloat() * 10^-6 +end + +local function test_read(block_size) + local start_us = micros() + + local file = assert(io.open(name, "r")) + + local i = 0 + while i < SIZE do + local data = file:read(block_size) + if data ~= nil then + i = i + #data + end + end + assert(i == SIZE) + + file:close() + + return (micros() - start_us):tofloat() * 10^-6 +end + +local block_index = 0 +local function update() + + block_index = block_index + 1 + if block_index > #BLOCKS then + -- Done + return + end + + local write = { + min = nil, + max = nil, + average = 0, + } + local read = { + min = nil, + max = nil, + average = 0, + } + + for _ = 1, tests do + local write_dt = test_write(BLOCKS[block_index]) + if (write.min == nil) or (write_dt < write.min) then + write.min = write_dt + end + if (write.max == nil) or (write_dt > write.max) then + write.max = write_dt + end + write.average = write.average + write_dt + + local read_dt = test_read(BLOCKS[block_index]) + if (read.min == nil) or (read_dt < read.min) then + read.min = read_dt + end + if (read.max == nil) or (read_dt > read.max) then + read.max = read_dt + end + read.average = read.average + read_dt + end + + print(string.format("Block size: %i", BLOCKS[block_index])) + print(string.format("Read: %f, %f, %f", read.min, read.average / tests, read.max)) + print(string.format("Write: %f, %f, %f", write.min, write.average / tests, write.max)) + + return update, 0 +end + +return update, 1000 diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index b4f002d52e6fab..3c6c8b749a7cb6 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -284,6 +284,12 @@ const AP_Param::GroupInfo AP_Vehicle::var_info[] = { AP_SUBGROUPINFO(serial_manager, "SERIAL", 31, AP_Vehicle, AP_SerialManager), #endif +#if AP_NETWORKING_FILESYSTEM_ENABLED + // @Group: NET_9P2K_ + // @Path: ../AP_Networking/AP_Networking_NineP2000.cpp + AP_SUBGROUPINFO(networking.NineP2000_client, "NET_9P2K_", 32, AP_Vehicle, NineP2000), +#endif + AP_GROUPEND };