/* 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) { 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.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; } } 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 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; // for future use 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 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; if (c.token_ofs == 0) { c.idx = 0; ap = AP_Param::first(&c.token, &ptype); uint16_t idx = 0; while (idx < r.start && ap) { ap = AP_Param::next_scalar(&c.token, &ptype); idx++; } } else { c.idx++; ap = AP_Param::next_scalar(&c.token, &ptype); } 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++; } const uint8_t name_len = strlen(pname); const uint8_t type_len = AP_Param::type_size(ptype); uint8_t packed_len = type_len + name_len + 2; const uint8_t flags = 0; /* 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); 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); 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; }