Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/AP_Filesystem/AP_Filesystem_Param.cpp

665 lines
18 KiB
C++
Raw Blame History

/*
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_config.h"
#if AP_FILESYSTEM_PARAM_ENABLED
#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;
// QURT HAL already has a declaration of errno in errno.h
#if CONFIG_HAL_BOARD != HAL_BOARD_QURT
extern int errno;
#endif
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_NOTHROW 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_NOTHROW 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)) {
if (r.count == 0 && c.idx != AP_Param::count_parameters()) {
// the parameter count is incorrect, invalidate so a
// repeated param download avoids an error
AP_Param::invalidate_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 size estimation to avoid needing to scan entire file
stbuf->st_size = AP_Param::count_parameters() * 12;
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;
}
#endif // AP_FILESYSTEM_PARAM_ENABLED
Reference in New Issue View Git Blame Copy Permalink