ardupilot/libraries/AP_Filesystem/AP_Filesystem_Param.cpp

/*
   This program is free software: you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
/*
  ArduPilot filesystem interface for parameters
 */
#include "AP_Filesystem.h"
#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"

extern const AP_HAL::HAL& hal;
extern int errno;

int AP_Filesystem_Param::open(const char *fname, int flags, bool allow_absolute_path)
{
    if (!check_file_name(fname)) {
        errno = ENOENT;
        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) {
            break;
        }
    }
    if (idx == max_open_file) {
        errno = ENFILE;
        return -1;
    }
    struct rfile &r = file[idx];
    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.with_defaults = false;
    r.start = 0;
    r.count = 0;
    r.read_size = 0;
    r.file_size = 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
     */
    const char *c = strchr(fname, '?');
    while (c && *c) {
        c++;
        if (strncmp(c, "start=", 6) == 0) {
            uint32_t v = strtoul(c+6, nullptr, 10);
            if (v >= UINT16_MAX) {
                goto failed;
            }
            r.start = v;
            c += 6;
            c = strchr(c, '&');
            continue;
        }
        if (strncmp(c, "count=", 6) == 0) {
            uint32_t v = strtoul(c+6, nullptr, 10);
            if (v >= UINT16_MAX) {
                goto failed;
            }
            r.count = v;
            c += 6;
            c = strchr(c, '&');
            continue;
        }
#if AP_PARAM_DEFAULTS_ENABLED
        if (strncmp(c, "withdefaults=", 13) == 0) {
            uint32_t v = strtoul(c+13, nullptr, 10);
            if (v > 1) {
                goto failed;
            }
            r.with_defaults = v == 1;
            c += 13;
            c = strchr(c, '&');
            continue;
        }
#endif
    }

    return idx;

failed:
    delete [] r.cursors;
    r.open = false;
    errno = EINVAL;
    return -1;
}

int AP_Filesystem_Param::close(int fd)
{
    if (fd < 0 || fd >= max_open_file || !file[fd].open) {
        errno = EBADF;
        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;
    r.cursors = nullptr;
    delete r.writebuf;
    r.writebuf = nullptr;
    return ret;
}

/*
  packed format:
    file header:
      uint16_t magic = 0x671b or  0x671c for included default values
      uint16_t num_params
      uint16_t total_params

    per-parameter:

    uint8_t type:4;         // AP_Param type NONE=0, INT8=1, INT16=2, INT32=3, FLOAT=4
    uint8_t flags:4;        // bit 0: includes default value for this param
    uint8_t common_len:4;   // number of name bytes in common with previous entry, 0..15
    uint8_t name_len:4;     // non-common length of param name -1 (0..15)
    uint8_t name[name_len]; // name
    uint8_t data[];         // value, length given by variable type, data length doubled if default is included

    Any leading zero bytes after the header should be discarded as pad
    bytes. Pad bytes are used to ensure that a parameter data[] field
    does not cross a read packet boundary
 */

/*
  pack a single parameter. The buffer must be at least of size max_pack_len
 */
uint8_t AP_Filesystem_Param::pack_param(const struct rfile &r, struct cursor &c, uint8_t *buf)
{
    char name[AP_MAX_NAME_SIZE+1];
    name[AP_MAX_NAME_SIZE] = 0;
    enum ap_var_type ptype;
    AP_Param *ap;
    float default_val;

    if (c.token_ofs == 0) {
        c.idx = 0;
        ap = AP_Param::first(&c.token, &ptype, &default_val);
        uint16_t idx = 0;
        while (idx < r.start && ap) {
            ap = AP_Param::next_scalar(&c.token, &ptype, &default_val);
            idx++;
        }
    } else {
        c.idx++;
        ap = AP_Param::next_scalar(&c.token, &ptype, &default_val);
    }
    if (ap == nullptr || (r.count && c.idx >= r.count)) {
        return 0;
    }
    ap->copy_name_token(c.token, name, AP_MAX_NAME_SIZE, true);

    uint8_t common_len = 0;
    const char *last_name = c.last_name;
    const char *pname = name;
    while (*pname == *last_name && *pname) {
        common_len++;
        pname++;
        last_name++;
    }
    uint8_t name_len = strlen(pname);
    if (name_len == 0) {
        name_len = 1;
        common_len--;
        pname--;
    }
#if AP_PARAM_DEFAULTS_ENABLED
    const bool add_default = r.with_defaults && !is_equal(ap->cast_to_float(ptype), default_val);
#else
    const bool add_default = false;
#endif
    const uint8_t type_len = AP_Param::type_size(ptype);
    uint8_t packed_len = type_len + name_len + 2 + (add_default ? type_len : 0);
    const uint8_t flags = add_default;

    /*
      see if we need to add padding to ensure that a data field never
      crosses a block boundary. This ensures that re-reading a block
      won't get a corrupt value for a parameter
     */
    if (type_len > 1) {
        const uint32_t ofs = c.token_ofs + sizeof(struct header) + packed_len;
        const uint32_t ofs_mod = ofs % r.read_size;
        if (ofs_mod > 0 && ofs_mod < type_len) {
            const uint8_t pad = type_len - ofs_mod;
            memset(buf, 0, pad);
            buf += pad;
            packed_len += pad;
        }
    }

    buf[0] = uint8_t(ptype) | (flags<<4);
    buf[1] = common_len | ((name_len-1)<<4);
    memcpy(&buf[2], pname, name_len);
    memcpy(&buf[2+name_len], ap, type_len);
#if AP_PARAM_DEFAULTS_ENABLED
    if (add_default) {
        switch (ptype) {
            case AP_PARAM_NONE:
            case AP_PARAM_GROUP:
                // should never happen...
                break;
            case AP_PARAM_INT8: {
                const int32_t int8_default = default_val;
                memcpy(&buf[2+name_len+type_len], &int8_default, type_len);
                break;
            }
            case AP_PARAM_INT16: {
                const int16_t int16_default = default_val;
                memcpy(&buf[2+name_len+type_len], &int16_default, type_len);
                break;
            }
            case AP_PARAM_INT32: {
                const int32_t int32_default = default_val;
                memcpy(&buf[2+name_len+type_len], &int32_default, type_len);
                break;
            }
            case AP_PARAM_FLOAT:
            case AP_PARAM_VECTOR3F: {
                memcpy(&buf[2+name_len+type_len], &default_val, type_len);
                break;
            }
        }
    }
#endif

    strcpy(c.last_name, name);

    return packed_len;
}

/*
  seek the token to match file offset
 */
bool AP_Filesystem_Param::token_seek(const struct rfile &r, const uint32_t data_ofs, struct cursor &c)
{
    if (data_ofs == 0) {
        memset(&c, 0, sizeof(c));
        return true;
    }
    if (c.token_ofs > data_ofs) {
        memset(&c, 0, sizeof(c));
    }

    if (c.trailer_len > 0) {
        uint8_t n = MIN(c.trailer_len, data_ofs - c.token_ofs);
        if (n != c.trailer_len) {
            memmove(&c.trailer[0], &c.trailer[n], c.trailer_len - n);
        }
        c.trailer_len -= n;
        c.token_ofs += n;
    }
    
    while (data_ofs != c.token_ofs) {
        uint32_t available = data_ofs - c.token_ofs;
        uint8_t tbuf[max_pack_len];
        uint8_t len = pack_param(r, c, tbuf);
        if (len == 0) {
            // no more parameters
            break;
        }
        uint8_t n = MIN(len, available);
        if (len > available) {
            c.trailer_len = len - available;
            memcpy(c.trailer, &tbuf[n], c.trailer_len);
        }
        c.token_ofs += n;
    }
    return data_ofs == c.token_ofs;
}

int32_t AP_Filesystem_Param::read(int fd, 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) {
        // no read on upload
        errno = EINVAL;
        return -1;
    }
    size_t header_total = 0;

    /*
      we only allow for a single read size. This ensures that pad
      bytes placed to avoid a data value crossing a block boundary in
      the ftp protocol do not change when filling in lost packets
     */
    if (r.read_size == 0 && count > 0) {
        r.read_size = count;
    }
    if (r.read_size != 0 && r.read_size != count) {
        errno = EINVAL;
        return -1;
    }

    if (r.file_size != 0) {
        // ensure we don't try to read past EOF
        if (r.file_ofs > r.file_size) {
            count = 0;
        } else {
            count = MIN(count, r.file_size - r.file_ofs);
        }
    }

    if (r.file_ofs < sizeof(struct header)) {
        struct header hdr;
        hdr.total_params = AP_Param::count_parameters();
        if (hdr.total_params <= r.start) {
            errno = EINVAL;
            return -1;
        }
        hdr.num_params = hdr.total_params - r.start;
        if (r.count > 0 && hdr.num_params > r.count) {
            hdr.num_params = r.count;
        }
        uint8_t n = MIN(sizeof(hdr) - r.file_ofs, count);
        if (r.with_defaults) {
            hdr.magic = pmagic_with_default;
        }
        const uint8_t *b = (const uint8_t *)&hdr;
        memcpy(buf, &b[r.file_ofs], n);
        count -= n;
        header_total += n;
        r.file_ofs += n;
        buf = (void *)(n + (const uint8_t *)buf);
        if (count == 0) {
            return header_total;
        }
    }

    uint32_t data_ofs = r.file_ofs - sizeof(struct header);
    uint8_t best_i = 0;
    uint32_t best_ofs = r.cursors[0].token_ofs;
    size_t total = 0;

    // find the first cursor that is positioned after the file offset
    for (uint8_t i=1; i<num_cursors; i++) {
        struct cursor &c = r.cursors[i];
        if (c.token_ofs >= data_ofs && c.token_ofs < best_ofs) {
            best_i = i;
            best_ofs = c.token_ofs;
        }
    }
    struct cursor &c = r.cursors[best_i];

    if (data_ofs != c.token_ofs) {
        if (!token_seek(r, data_ofs, c)) {
            // must be EOF
            return header_total;
        }
    }
    if (count == 0) {
        return header_total;
    }
    uint8_t *ubuf = (uint8_t *)buf;

    if (c.trailer_len > 0) {
        uint8_t n = MIN(c.trailer_len, count);
        memcpy(ubuf, c.trailer, n);
        count -= n;
        ubuf += n;
        if (n != c.trailer_len) {
            memmove(&c.trailer[0], &c.trailer[n], c.trailer_len - n);
        }
        c.trailer_len -= n;
        total += n;
        c.token_ofs += n;
    }

    while (count > 0) {
        uint8_t tbuf[max_pack_len];
        uint8_t len = pack_param(r, c, tbuf);
        if (len == 0) {
            // no more params, use this to trigger EOF in later reads
            const uint32_t size = r.file_ofs + total;
            if (r.file_size == 0) {
                r.file_size = size;
            } else {
                r.file_size = MIN(size, r.file_size);
            }
            break;
        }
        uint8_t n = MIN(len, count);
        if (len > count) {
            c.trailer_len = len - count;
            memcpy(c.trailer, &tbuf[count], c.trailer_len);
        }
        memcpy(ubuf, tbuf, n);
        count -= n;
        ubuf += n;
        total += n;
        c.token_ofs += n;
    }
    r.file_ofs += total;
    return total + header_total;
}

int32_t AP_Filesystem_Param::lseek(int fd, int32_t offset, int seek_from)
{
    if (fd < 0 || fd >= max_open_file || !file[fd].open) {
        errno = EBADF;
        return -1;
    }
    struct rfile &r = file[fd];
    switch (seek_from) {
    case SEEK_SET:
        r.file_ofs = offset;
        break;
    case SEEK_CUR:
        r.file_ofs += offset;
        break;
    case SEEK_END:
        errno = EINVAL;
        return -1;
    }
    return r.file_ofs;
}

int AP_Filesystem_Param::stat(const char *name, struct stat *stbuf)
{
    if (!check_file_name(name)) {
        errno = ENOENT;
        return -1;
    }
    memset(stbuf, 0, sizeof(*stbuf));
    // give fixed size to avoid needing to scan entire file
    stbuf->st_size = 1024*1024;
    return 0;
}

/*
  check for the right file name
 */
bool AP_Filesystem_Param::check_file_name(const char *name)
{
    const uint8_t packed_len = strlen(PACKED_NAME);
    if (strncmp(name, PACKED_NAME, packed_len) == 0 &&
        (name[packed_len] == 0 || name[packed_len] == '?')) {
        return true;
    }
    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;
}