diff --git a/apps/px4/tests/tests_param.c b/apps/px4/tests/tests_param.c index e564bda8ff..6a4a16f515 100644 --- a/apps/px4/tests/tests_param.c +++ b/apps/px4/tests/tests_param.c @@ -52,19 +52,29 @@ test_param(int argc, char *argv[]) p = param_find("test"); if (p == PARAM_INVALID) - errx(1, "test parameter not found\n"); + errx(1, "test parameter not found"); param_type_t t = param_type(p); if (t != PARAM_TYPE_INT32) - errx(1, "test parameter type mismatch (got %u)\n", (unsigned)t); + errx(1, "test parameter type mismatch (got %u)", (unsigned)t); int32_t val; if (param_get(p, &val) != 0) - errx(1, "failed to read test parameter\n"); + errx(1, "failed to read test parameter"); if (val != 0x12345678) - errx(1, "parameter value mismatch\n"); + errx(1, "parameter value mismatch"); - warnx("parameter test PASS\n"); + val = 0xa5a5a5a5; + if (param_set(p, &val) != 0) + errx(1, "failed to write test parameter"); + if (param_get(p, &val) != 0) + errx(1, "failed to re-read test parameter"); + if (val != 0xa5a5a5a5) + errx(1, "parameter value mismatch after write"); + + param_export(NULL); + + warnx("parameter test PASS"); return 0; } diff --git a/apps/systemlib/Makefile b/apps/systemlib/Makefile index fd6846937f..1a1bab0765 100644 --- a/apps/systemlib/Makefile +++ b/apps/systemlib/Makefile @@ -38,7 +38,9 @@ CSRCS = err.c \ hx_stream.c \ perf_counter.c \ - param/param.c + param/param.c \ + bson/bson.c \ + bson/bcon.c # # XXX this really should be a CONFIG_* test diff --git a/apps/systemlib/param/param.c b/apps/systemlib/param/param.c index bb82941a0c..315ba187bc 100644 --- a/apps/systemlib/param/param.c +++ b/apps/systemlib/param/param.c @@ -6,10 +6,13 @@ #include #include - +#include #include +#include -#include "param.h" +#include "systemlib/param/param.h" +#include "systemlib/uthash/utarray.h" +#include "systemlib/bson/bson.h" /** * Array of static parameter info. @@ -19,12 +22,78 @@ static const struct param_info_s *param_info_base = (struct param_info_s *)&__pa static const struct param_info_s *param_info_limit = (struct param_info_s *)&__param_end; #define param_info_count ((unsigned)(param_info_limit - param_info_base)) +/** + * Storage for modified parameters. + */ +struct param_wbuf_s +{ + param_t param; + union param_value_u val; + bool unsaved; +}; + +UT_icd param_icd = {sizeof(struct param_wbuf_s), NULL, NULL, NULL}; +UT_array *param_values; + +static void +param_lock(void) +{ + /* XXX */ +} + +static void +param_unlock(void) +{ + /* XXX */ +} + +static void +param_assert_locked(void) +{ + /* XXX */ +} + static bool handle_in_range(param_t handle) { return (handle < param_info_count); } +static int +param_compare_values(const void *a, const void *b) +{ + struct param_wbuf_s *pa = (struct param_wbuf_s *)a; + struct param_wbuf_s *pb = (struct param_wbuf_s *)b; + + if (pa->param < pb->param) + return -1; + if (pa->param > pb->param) + return 1; + return 0; +} + +static struct param_wbuf_s * +param_find_changed(param_t param) +{ + struct param_wbuf_s *s = NULL; + + param_assert_locked(); + + if (param_values != NULL) { +#if 0 /* utarray_find requires bsearch, not available */ + struct param_wbuf_s key; + key.param = param; + s = utarray_find(param_values, &key, param_compare_values); +#else + while ((s = (struct param_wbuf_s *)utarray_next(param_values, s)) != NULL) { + if (s->param == param) + break; + } +#endif + } + return s; +} + param_t param_find(const char *name) { @@ -39,8 +108,17 @@ param_find(const char *name) return PARAM_INVALID; } -enum -param_type_e param_type(param_t param) +const char * +param_name(param_t param) +{ + if (handle_in_range(param)) + return param_info_base[param].name; + + return NULL; +} + +enum param_type_e +param_type(param_t param) { if (handle_in_range(param)) return param_info_base[param].type; @@ -51,7 +129,6 @@ param_type_e param_type(param_t param) size_t param_size(param_t param) { - if (handle_in_range(param)) { switch (param_info_base[param].type) { case PARAM_TYPE_INT32: @@ -72,39 +149,193 @@ param_size(param_t param) int param_get(param_t param, void *val) { + int result = -1; + + param_lock(); + if (handle_in_range(param)) { + struct param_wbuf_s *s = param_find_changed(param); + const union param_value_u *v; + + /* work out whether we're fetching the default or a written value */ + if (s != NULL) { + v = &s->val; + } else { + v = ¶m_info_base[param].val; + } + /* XXX look for updated value stored elsewhere */ switch (param_info_base[param].type) { case PARAM_TYPE_INT32: - *(int32_t *)val = param_info_base[param].i; - return 0; + *(int32_t *)val = v->i; + result = 0; + break; case PARAM_TYPE_FLOAT: - *(float *)val = param_info_base[param].f; - return 0; + *(float *)val = v->f; + result = 0; + break; case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: - memcpy(val, param_info_base[param].p, param_size(param)); - return 0; + *(void **)val = v->p; + result = 0; + break; default: - return -1; + break; } + } - return -1; + + param_unlock(); + + return result; } int param_set(param_t param, void *val) { + int result = -1; + + param_lock(); + + if (param_values == NULL) + utarray_new(param_values, ¶m_icd); + if (param_values == NULL) + goto out; + if (handle_in_range(param)) { - /* XXX maintain list of changed values */ + struct param_wbuf_s *s = param_find_changed(param); + if (s == NULL) { + + /* construct a new parameter */ + struct param_wbuf_s buf = { .param = param }; + + /* add it to the array and sort */ + utarray_push_back(param_values, &buf); + utarray_sort(param_values, param_compare_values); + + /* find it after sorting */ + s = param_find_changed(param); + } + + /* update the changed value */ + switch (param_info_base[param].type) { + case PARAM_TYPE_INT32: + s->val.i = *(int32_t *)val; + break; + + case PARAM_TYPE_FLOAT: + s->val.f = *(float *)val; + break; + + case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: + s->val.p = *(void **)val; + break; + + default: + goto out; + } + s->unsaved = true; + + result = 0; } - return -1; - +out: + param_unlock(); + return result; } +int +param_export(const char *filename, bool only_unsaved) +{ + struct param_wbuf_s *s; + bson b[1]; + int result = -1; + + param_lock(); + + bson_init(b); + + while ((s = (struct param_wbuf_s *)utarray_next(param_values, s)) != NULL) { + const struct param_info_s *is = ¶m_info_base[s->param]; + + /* + * if we are only saving values changed since last save, and this + * one hasn't, then skip it + */ + if (only_unsaved & !s->unsaved) + continue; + s->unsaved = false; + + switch (is->type) { + case PARAM_TYPE_INT32: + if (bson_append_int(b, is->name, s->val.i) != BSON_OK) + goto out; + break; + + case PARAM_TYPE_FLOAT: + if (bson_append_double(b, is->name, s->val.f) != BSON_OK) + goto out; + break; + + case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: + if (bson_append_binary(b, + is->name, + is->type, + is->val.p, + is->type - PARAM_TYPE_STRUCT) != BSON_OK) + goto out; + break; + default: + goto out; + } + } + result = 0; + +out: + param_unlock(); + + if (result == 0) { + bson_finish(b); + bson_print(b); + + warnx("object is %d bytes", bson_buffer_size(b)); + + if (filename != NULL) { + int fd = open(filename, O_CREAT | O_WRONLY | O_TRUNC, 0666); + int siz = bson_buffer_size(b); + int len = write(fd, bson_data(b), siz); + close(fd); + if (len != siz) + result = -1; + } + } + bson_destroy(b); + + return result; +} + +int +param_import(const char *filename) +{ + return -1; +} + +void +param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_changed) +{ + param_t param; + + for (param = 0; handle_in_range(param); param++) { + + /* if requested, skip unchanged values */ + if (only_changed && (param_find_changed(param) == NULL)) + continue; + + func(arg, param); + } +} \ No newline at end of file diff --git a/apps/systemlib/param/param.h b/apps/systemlib/param/param.h index 9f7808f30a..0d94ce6e32 100644 --- a/apps/systemlib/param/param.h +++ b/apps/systemlib/param/param.h @@ -8,6 +8,7 @@ #define _SYSTEMLIB_PARAM_PARAM_H #include +#include /** * Parameter types. @@ -46,10 +47,18 @@ typedef uintptr_t param_t; */ __EXPORT param_t param_find(const char *name); +/** + * Obtain the name of a parameter. + * + * @param param A handle returned by param_find or passed by param_foreach. + * @return The name assigned to the parameter, or NULL if the handle is invalid. + */ +__EXPORT const char *param_name(param_t param); + /** * Obtain the type of a parameter. * - * @param param A handle returned by param_find. + * @param param A handle returned by param_find or passed by param_foreach. * @return The type assigned to the parameter. */ __EXPORT param_type_t param_type(param_t param); @@ -57,7 +66,7 @@ __EXPORT param_type_t param_type(param_t param); /** * Determine the size of a parameter. * - * @param param A handle returned by param_find. + * @param param A handle returned by param_find or passed by param_foreach. * @return The size of the parameter's value. */ __EXPORT size_t param_size(param_t param); @@ -65,7 +74,7 @@ __EXPORT size_t param_size(param_t param); /** * Obtain the scalar value of a parameter. * - * @param param A handle returned by param_find. + * @param param A handle returned by param_find or passed by param_foreach. * @param val Where to return the value, assumed to point to suitable storage for the parameter type. * For structures, a pointer to the structure is returned. * @return Zero if the parameter's value could be returned as a scalar, nonzero otherwise. @@ -75,13 +84,43 @@ __EXPORT int param_get(param_t param, void *val); /** * Set the scalar value of a parameter. * - * @param param A handle returned by param_find. + * @param param A handle returned by param_find or passed by param_foreach. * @param val The value to set; assumed to point to a variable of the parameter type. * For structures, the pointer is assumed to point to a copy of the structure. * @return Zero if the parameter's value could be set from a scalar, nonzero otherwise. */ __EXPORT int param_set(param_t param, void *val); +/** + * Export changed parameters to a file. + * + * @param filename The name of the file to export to. If it exists, it will be overwritten. + * @param only_unsaved Only export changed parameters that have not yet been exported. + * @return Zero on success, nonzero on failure. + */ +__EXPORT int param_export(const char *filename, bool only_unsaved); + +/** + * Import parameters from a file, discarding any unrecognised parameters. + * + * @param filename The name of the file to import from. + * @return Zero on success, nonzero if an error occurred during import. + * Note that in the failure case, parameters may be inconsistent. + */ +__EXPORT int param_import(const char *filename); + +/** + * Apply a function to each parameter. + * + * Note that the parameter set is not locked during the traversal. + * + * @param func The function to invoke for each parameter. + * @param arg Argument passed to the function. + * @param only_changed If true, the function is only called for parameters whose values have + * been changed from the default. + */ +__EXPORT void param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_changed); + /* * Macros creating static parameter definitions. * @@ -97,7 +136,7 @@ __EXPORT int param_set(param_t param, void *val); struct param_info_s __param__##_name = { \ .name = #_name, \ .type = PARAM_TYPE_INT32, \ - .i = _default \ + .val.i = _default \ } /** define a float parameter */ @@ -107,7 +146,7 @@ __EXPORT int param_set(param_t param, void *val); struct param_info_s __param__##_name = { \ .name = #_name, \ .type = PARAM_TYPE_FLOAT, \ - .f = _default \ + .val.f = _default \ } /** define a parameter that points to a structure */ @@ -117,9 +156,19 @@ __EXPORT int param_set(param_t param, void *val); struct param_info_s __param__##_name = { \ .name = #_name, \ .type = PARAM_TYPE_STRUCT + sizeof(_default), \ - .p = &_default; \ + .val.p = &_default; \ } +/** + * Parameter value union. + */ +union param_value_u +{ + void *p; + int32_t i; + float f; +}; + /** * Static parameter definition structure. * @@ -130,11 +179,7 @@ struct param_info_s { const char *name; param_type_t type; - union { - void *p; - int32_t i; - float f; - }; + union param_value_u val; }; #endif