From b941b8c93a2d8d9256b1e1c5f16be002b89cc8dd Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 8 Apr 2021 11:11:29 +1000 Subject: [PATCH] AP_Filesystem: allow for parameter upload via ftp --- .../AP_Filesystem/AP_Filesystem_Mission.cpp | 2 +- .../AP_Filesystem/AP_Filesystem_Param.cpp | 205 +++++++++++++++++- libraries/AP_Filesystem/AP_Filesystem_Param.h | 13 +- 3 files changed, 209 insertions(+), 11 deletions(-) diff --git a/libraries/AP_Filesystem/AP_Filesystem_Mission.cpp b/libraries/AP_Filesystem/AP_Filesystem_Mission.cpp index f5d4886fc6..389f393232 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_Mission.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem_Mission.cpp @@ -337,7 +337,7 @@ bool AP_Filesystem_Mission::all_zero(const uint8_t *b, uint8_t len) const bool AP_Filesystem_Mission::finish_upload(const rfile &r) { const uint32_t flen = r.writebuf->get_length(); - uint8_t *b = (uint8_t *)r.writebuf->get_writeable_string(); + const uint8_t *b = (const uint8_t *)r.writebuf->get_string(); struct header hdr; if (flen < sizeof(hdr)) { return false; diff --git a/libraries/AP_Filesystem/AP_Filesystem_Param.cpp b/libraries/AP_Filesystem/AP_Filesystem_Param.cpp index 1455b1f4eb..b5e56e2926 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_Param.cpp +++ b/libraries/AP_Filesystem/AP_Filesystem_Param.cpp @@ -19,6 +19,7 @@ #include "AP_Filesystem_Param.h" #include #include +#include #define PACKED_NAME "param.pck" @@ -31,9 +32,7 @@ int AP_Filesystem_Param::open(const char *fname, int flags) errno = ENOENT; return -1; } - if ((flags & O_ACCMODE) != O_RDONLY) { - return -1; - } + bool read_only = ((flags & O_ACCMODE) == O_RDONLY); uint8_t idx; for (idx=0; idx= max_open_file || !file[fd].open) { + errno = EBADF; + return -1; + } + struct rfile &r = file[fd]; + if (r.writebuf == nullptr) { + errno = EBADF; + return -1; + } + struct header hdr; + if (r.file_ofs == 0 && count >= sizeof(hdr)) { + // pre-expand the buffer to the full size when we get the header + memcpy(&hdr, buf, sizeof(hdr)); + if (hdr.magic == pmagic) { + const uint32_t flen = hdr.total_params; + if (flen > r.writebuf->get_length()) { + if (!r.writebuf->append(nullptr, flen - r.writebuf->get_length())) { + // not enough memory + return -1; + } + } + } + } + if (r.file_ofs + count > r.writebuf->get_length()) { + if (!r.writebuf->append(nullptr, r.file_ofs + count - r.writebuf->get_length())) { + return -1; + } + } + uint8_t *b = (uint8_t *)r.writebuf->get_writeable_string(); + memcpy(&b[r.file_ofs], buf, count); + r.file_ofs += count; + return count; +} + +/* + parse incoming parameters + */ +bool AP_Filesystem_Param::param_upload_parse(const rfile &r, bool &need_retry) +{ + need_retry = false; + + const uint8_t *b = (const uint8_t *)r.writebuf->get_string(); + uint32_t length = r.writebuf->get_length(); + struct header hdr; + if (length < sizeof(hdr)) { + return false; + } + memcpy(&hdr, b, sizeof(hdr)); + if (hdr.magic != pmagic) { + return false; + } + if (length != hdr.total_params) { + return false; + } + b += sizeof(hdr); + + char last_name[17] {}; + + for (uint16_t i=0; i>4); + if (flags != 0) { + return false; + } + uint8_t common_len = b[1]&0xF; + uint8_t name_len = (b[1]>>4)+1; + if (common_len + name_len > 16) { + return false; + } + char name[17]; + memcpy(name, last_name, common_len); + memcpy(&name[common_len], &b[2], name_len); + name[common_len+name_len] = 0; + + memcpy(last_name, name, sizeof(name)); + enum ap_var_type ptype2 = AP_PARAM_NONE; + uint16_t flags2; + + b += 2 + name_len; + + AP_Param *p = AP_Param::find(name, &ptype2, &flags2); + if (p == nullptr) { + if (ptype == AP_PARAM_INT8) { + b++; + } else if (ptype == AP_PARAM_INT16) { + b += 2; + } else { + b += 4; + } + continue; + } + + /* + if we are enabling a subsystem we need a small delay between + parameters to allow main thread to perform any allocation of + backends + */ + bool need_delay = ((flags2 & AP_PARAM_FLAG_ENABLE) != 0 && + ptype2 == AP_PARAM_INT8 && + ((AP_Int8 *)p)->get() == 0); + + if (ptype == ptype2 && ptype == AP_PARAM_INT32) { + // special handling of int32_t to preserve all bits + int32_t v; + memcpy(&v, b, sizeof(v)); + ((AP_Int32 *)p)->set(v); + b += 4; + } else if (ptype == AP_PARAM_INT8) { + if (need_delay && b[0] == 0) { + need_delay = false; + } + p->set_float((int8_t)b[0], ptype2); + b += 1; + } else if (ptype == AP_PARAM_INT16) { + int16_t v; + memcpy(&v, b, sizeof(v)); + p->set_float(float(v), ptype2); + b += 2; + } else if (ptype == AP_PARAM_INT32) { + int32_t v; + memcpy(&v, b, sizeof(v)); + p->set_float(float(v), ptype2); + b += 4; + } else if (ptype == AP_PARAM_FLOAT) { + float v; + memcpy(&v, b, sizeof(v)); + p->set_float(v, ptype2); + b += 4; + } + + p->save_sync(false, false); + + if (need_delay) { + // let main thread have some time to init backends + need_retry = true; + hal.scheduler->delay(100); + } + } + return true; +} + + +/* + parse incoming parameters + */ +bool AP_Filesystem_Param::finish_upload(const rfile &r) +{ + uint8_t loops = 0; + while (loops++ < 4) { + bool need_retry; + if (!param_upload_parse(r, need_retry)) { + return false; + } + if (!need_retry) { + break; + } + } + return true; +} diff --git a/libraries/AP_Filesystem/AP_Filesystem_Param.h b/libraries/AP_Filesystem/AP_Filesystem_Param.h index 6476cfdb39..98e8dc6af1 100644 --- a/libraries/AP_Filesystem/AP_Filesystem_Param.h +++ b/libraries/AP_Filesystem/AP_Filesystem_Param.h @@ -16,6 +16,7 @@ #pragma once #include "AP_Filesystem_backend.h" +#include #include @@ -28,6 +29,7 @@ public: int32_t read(int fd, void *buf, uint32_t count) override; int32_t lseek(int fd, int32_t offset, int whence) override; int stat(const char *pathname, struct stat *stbuf) override; + int32_t write(int fd, const void *buf, uint32_t count) override; private: // we maintain two cursors per open file to minimise seeking @@ -40,11 +42,13 @@ private: // maximum size of one packed parameter static constexpr uint8_t max_pack_len = AP_MAX_NAME_SIZE + 2 + 4 + 3; + static constexpr uint16_t pmagic = 0x671b; + // header at front of the file struct header { - uint16_t magic = 0x671b; + uint16_t magic = pmagic; uint16_t num_params; - uint16_t total_params; + uint16_t total_params; // for upload this is total file length }; struct cursor { @@ -64,9 +68,14 @@ private: uint32_t file_ofs; uint32_t file_size; struct cursor *cursors; + ExpandingString *writebuf; // for upload } file[max_open_file]; bool token_seek(const struct rfile &r, const uint32_t data_ofs, struct cursor &c); uint8_t pack_param(const struct rfile &r, struct cursor &c, uint8_t *buf); bool check_file_name(const char *fname); + + // finish uploading parameters + bool finish_upload(const rfile &r); + bool param_upload_parse(const rfile &r, bool &need_retry); };