AP_Filesystem: allow for parameter upload via ftp

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;

libraries/AP_Filesystem/AP_Filesystem_Param.cpp

@@ -19,6 +19,7 @@
 #include "AP_Filesystem_Param.h"
 #include <AP_Param/AP_Param.h>
 #include <AP_Math/AP_Math.h>
+#include <ctype.h>
 
 #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; idx++) {
         if (!file[idx].open) {
@@ -45,15 +44,27 @@ int AP_Filesystem_Param::open(const char *fname, int flags)
         return -1;
     }
     struct rfile &r = file[idx];
-    r.cursors = new cursor[num_cursors];
-    if (r.cursors == nullptr) {
-        errno = ENOMEM;
-        return -1;
+    if (read_only) {
+        r.cursors = new cursor[num_cursors];
+        if (r.cursors == nullptr) {
+            errno = ENOMEM;
+            return -1;
+        }
     }
     r.file_ofs = 0;
     r.open = true;
     r.start = 0;
     r.count = 0;
+    r.writebuf = nullptr;
+    if (!read_only) {
+        // setup for upload
+        r.writebuf = new ExpandingString();
+        if (r.writebuf == nullptr) {
+            close(idx);
+            errno = ENOMEM;
+            return -1;
+        }
+    }
 
     /*
       allow for URI style arguments param.pck?start=N&count=C
@@ -99,9 +110,17 @@ int AP_Filesystem_Param::close(int fd)
         return -1;
     }
     struct rfile &r = file[fd];
+    int ret = 0;
+    if (r.writebuf != nullptr && !finish_upload(r)) {
+        errno = EINVAL;
+        ret = -1;
+    }
     r.open = false;
     delete [] r.cursors;
-    return 0;
+    r.cursors = nullptr;
+    delete r.writebuf;
+    r.writebuf = nullptr;
+    return ret;
 }
 
 /*
@@ -239,6 +258,11 @@ int32_t AP_Filesystem_Param::read(int fd, void *buf, uint32_t count)
     }
 
     struct rfile &r = file[fd];
+    if (r.writebuf != nullptr) {
+        // no read on upload
+        errno = EINVAL;
+        return -1;
+    }
     size_t header_total = 0;
 
     /*
@@ -398,3 +422,168 @@ bool AP_Filesystem_Param::check_file_name(const char *name)
     }
     return false;
 }
+
+/*
+  support param upload
+ */
+int32_t AP_Filesystem_Param::write(int fd, const void *buf, uint32_t count)
+{
+    if (fd < 0 || fd >= 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<hdr.num_params; i++) {
+        enum ap_var_type ptype = (enum ap_var_type)(b[0]&0x0F);
+        uint8_t flags = (enum ap_var_type)(b[0]>>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;
+}

libraries/AP_Filesystem/AP_Filesystem_Param.h

@@ -16,6 +16,7 @@
 #pragma once
 
 #include "AP_Filesystem_backend.h"
+#include <AP_Common/ExpandingString.h>
 
 #include <AP_Param/AP_Param.h>
 
@@ -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);
 };