diff options
Diffstat (limited to 'src/modules/systemlib/param/param.c')
-rw-r--r-- | src/modules/systemlib/param/param.c | 805 |
1 files changed, 805 insertions, 0 deletions
diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c new file mode 100644 index 000000000..69a9bdf9b --- /dev/null +++ b/src/modules/systemlib/param/param.c @@ -0,0 +1,805 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file param.c + * + * Global parameter store. + * + * Note that it might make sense to convert this into a driver. That would + * offer some interesting options regarding state for e.g. ORB advertisements + * and background parameter saving. + */ + +#include <debug.h> +#include <string.h> +#include <stdbool.h> +#include <fcntl.h> +#include <unistd.h> +#include <systemlib/err.h> +#include <errno.h> + +#include <sys/stat.h> + +#include <drivers/drv_hrt.h> + +#include "systemlib/param/param.h" +#include "systemlib/uthash/utarray.h" +#include "systemlib/bson/tinybson.h" + +#include "uORB/uORB.h" +#include "uORB/topics/parameter_update.h" + +#if 1 +# define debug(fmt, args...) do { warnx(fmt, ##args); } while(0) +#else +# define debug(fmt, args...) do { } while(0) +#endif + +/** + * Array of static parameter info. + */ +extern char __param_start, __param_end; +static const struct param_info_s *param_info_base = (struct param_info_s *) &__param_start; +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; +}; + +/** flexible array holding modified parameter values */ +UT_array *param_values; + +/** array info for the modified parameters array */ +const UT_icd param_icd = {sizeof(struct param_wbuf_s), NULL, NULL, NULL}; + +/** parameter update topic */ +ORB_DEFINE(parameter_update, struct parameter_update_s); + +/** parameter update topic handle */ +static orb_advert_t param_topic = -1; + +/** lock the parameter store */ +static void +param_lock(void) +{ + /* XXX */ +} + +/** unlock the parameter store */ +static void +param_unlock(void) +{ + /* XXX */ +} + +/** assert that the parameter store is locked */ +static void +param_assert_locked(void) +{ + /* XXX */ +} + +/** + * Test whether a param_t is value. + * + * @param param The parameter handle to test. + * @return True if the handle is valid. + */ +static bool +handle_in_range(param_t param) +{ + return (param < param_info_count); +} + +/** + * Compare two modifid parameter structures to determine ordering. + * + * This function is suitable for passing to qsort or bsearch. + */ +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; +} + +/** + * Locate the modified parameter structure for a parameter, if it exists. + * + * @param param The parameter being searched. + * @return The structure holding the modified value, or + * NULL if the parameter has not been modified. + */ +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; +} + +static void +param_notify_changes(void) +{ + struct parameter_update_s pup = { .timestamp = hrt_absolute_time() }; + + /* + * If we don't have a handle to our topic, create one now; otherwise + * just publish. + */ + if (param_topic == -1) { + param_topic = orb_advertise(ORB_ID(parameter_update), &pup); + + } else { + orb_publish(ORB_ID(parameter_update), param_topic, &pup); + } +} + +param_t +param_find(const char *name) +{ + param_t param; + + /* perform a linear search of the known parameters */ + for (param = 0; handle_in_range(param); param++) { + if (!strcmp(param_info_base[param].name, name)) + return param; + } + + /* not found */ + return PARAM_INVALID; +} + +unsigned +param_count(void) +{ + return param_info_count; +} + +param_t +param_for_index(unsigned index) +{ + if (index < param_info_count) + return (param_t)index; + + return PARAM_INVALID; +} + +int +param_get_index(param_t param) +{ + if (handle_in_range(param)) + return (unsigned)param; + + return -1; +} + +const char * +param_name(param_t param) +{ + if (handle_in_range(param)) + return param_info_base[param].name; + + return NULL; +} + +bool +param_value_is_default(param_t param) +{ + return param_find_changed(param) ? false : true; +} + +bool +param_value_unsaved(param_t param) +{ + static struct param_wbuf_s *s; + + s = param_find_changed(param); + + if (s && s->unsaved) + return true; + + return false; +} + +enum param_type_e +param_type(param_t param) +{ + if (handle_in_range(param)) + return param_info_base[param].type; + + return PARAM_TYPE_UNKNOWN; +} + +size_t +param_size(param_t param) +{ + if (handle_in_range(param)) { + switch (param_type(param)) { + case PARAM_TYPE_INT32: + case PARAM_TYPE_FLOAT: + return 4; + + case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: + /* decode structure size from type value */ + return param_type(param) - PARAM_TYPE_STRUCT; + + default: + return 0; + } + } + + return 0; +} + +/** + * Obtain a pointer to the storage allocated for a parameter. + * + * @param param The parameter whose storage is sought. + * @return A pointer to the parameter value, or NULL + * if the parameter does not exist. + */ +static const void * +param_get_value_ptr(param_t param) +{ + const void *result = NULL; + + param_assert_locked(); + + if (handle_in_range(param)) { + + const union param_value_u *v; + + /* work out whether we're fetching the default or a written value */ + struct param_wbuf_s *s = param_find_changed(param); + + if (s != NULL) { + v = &s->val; + + } else { + v = ¶m_info_base[param].val; + } + + if (param_type(param) == PARAM_TYPE_STRUCT) { + result = v->p; + + } else { + result = v; + } + } + + return result; +} + +int +param_get(param_t param, void *val) +{ + int result = -1; + + param_lock(); + + const void *v = param_get_value_ptr(param); + + if (val != NULL) { + memcpy(val, v, param_size(param)); + result = 0; + } + + param_unlock(); + + return result; +} + +static int +param_set_internal(param_t param, const void *val, bool mark_saved) +{ + int result = -1; + bool params_changed = false; + + param_lock(); + + if (param_values == NULL) + utarray_new(param_values, ¶m_icd); + + if (param_values == NULL) { + debug("failed to allocate modified values array"); + goto out; + } + + if (handle_in_range(param)) { + + struct param_wbuf_s *s = param_find_changed(param); + + if (s == NULL) { + + /* construct a new parameter */ + struct param_wbuf_s buf = { + .param = param, + .val.p = NULL, + .unsaved = false + }; + + /* 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_type(param)) { + 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: + if (s->val.p == NULL) { + s->val.p = malloc(param_size(param)); + + if (s->val.p == NULL) { + debug("failed to allocate parameter storage"); + goto out; + } + } + + memcpy(s->val.p, val, param_size(param)); + break; + + default: + goto out; + } + + s->unsaved = !mark_saved; + params_changed = true; + result = 0; + } + +out: + param_unlock(); + + /* + * If we set something, now that we have unlocked, go ahead and advertise that + * a thing has been set. + */ + if (params_changed) + param_notify_changes(); + + return result; +} + +int +param_set(param_t param, const void *val) +{ + return param_set_internal(param, val, false); +} + +void +param_reset(param_t param) +{ + struct param_wbuf_s *s = NULL; + + param_lock(); + + if (handle_in_range(param)) { + + /* look for a saved value */ + s = param_find_changed(param); + + /* if we found one, erase it */ + if (s != NULL) { + int pos = utarray_eltidx(param_values, s); + utarray_erase(param_values, pos, 1); + } + } + + param_unlock(); + + if (s != NULL) + param_notify_changes(); +} + +void +param_reset_all(void) +{ + param_lock(); + + if (param_values != NULL) { + utarray_free(param_values); + } + + /* mark as reset / deleted */ + param_values = NULL; + + param_unlock(); + + param_notify_changes(); +} + +static const char *param_default_file = "/eeprom/parameters"; +static char *param_user_file = NULL; + +int +param_set_default_file(const char* filename) +{ + if (param_user_file != NULL) { + free(param_user_file); + param_user_file = NULL; + } + if (filename) + param_user_file = strdup(filename); + return 0; +} + +const char * +param_get_default_file(void) +{ + return (param_user_file != NULL) ? param_user_file : param_default_file; +} + +int +param_save_default(void) +{ + /* delete the file in case it exists */ + unlink(param_get_default_file()); + + /* create the file */ + int fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL); + + if (fd < 0) { + warn("opening '%s' for writing failed", param_get_default_file()); + return -1; + } + + int result = param_export(fd, false); + close(fd); + + if (result != 0) { + warn("error exporting parameters to '%s'", param_get_default_file()); + unlink(param_get_default_file()); + return -2; + } + + return 0; +} + +/** + * @return 0 on success, 1 if all params have not yet been stored, -1 if device open failed, -2 if writing parameters failed + */ +int +param_load_default(void) +{ + int fd = open(param_get_default_file(), O_RDONLY); + + if (fd < 0) { + /* no parameter file is OK, otherwise this is an error */ + if (errno != ENOENT) { + warn("open '%s' for reading failed", param_get_default_file()); + return -1; + } + return 1; + } + + int result = param_load(fd); + close(fd); + + if (result != 0) { + warn("error reading parameters from '%s'", param_get_default_file()); + return -2; + } + + return 0; +} + +int +param_export(int fd, bool only_unsaved) +{ + struct param_wbuf_s *s = NULL; + struct bson_encoder_s encoder; + int result = -1; + + param_lock(); + + bson_encoder_init_file(&encoder, fd); + + /* no modified parameters -> we are done */ + if (param_values == NULL) { + result = 0; + goto out; + } + + while ((s = (struct param_wbuf_s *)utarray_next(param_values, s)) != NULL) { + + int32_t i; + float f; + + /* + * 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; + + /* append the appropriate BSON type object */ + switch (param_type(s->param)) { + case PARAM_TYPE_INT32: + param_get(s->param, &i); + + if (bson_encoder_append_int(&encoder, param_name(s->param), i)) { + debug("BSON append failed for '%s'", param_name(s->param)); + goto out; + } + + break; + + case PARAM_TYPE_FLOAT: + param_get(s->param, &f); + + if (bson_encoder_append_double(&encoder, param_name(s->param), f)) { + debug("BSON append failed for '%s'", param_name(s->param)); + goto out; + } + + break; + + case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: + if (bson_encoder_append_binary(&encoder, + param_name(s->param), + BSON_BIN_BINARY, + param_size(s->param), + param_get_value_ptr(s->param))) { + debug("BSON append failed for '%s'", param_name(s->param)); + goto out; + } + + break; + + default: + debug("unrecognized parameter type"); + goto out; + } + } + + result = 0; + +out: + param_unlock(); + + if (result == 0) + result = bson_encoder_fini(&encoder); + + return result; +} + +struct param_import_state { + bool mark_saved; +}; + +static int +param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node) +{ + float f; + int32_t i; + void *v, *tmp = NULL; + int result = -1; + struct param_import_state *state = (struct param_import_state *)private; + + /* + * EOO means the end of the parameter object. (Currently not supporting + * nested BSON objects). + */ + if (node->type == BSON_EOO) { + debug("end of parameters"); + return 0; + } + + /* + * Find the parameter this node represents. If we don't know it, + * ignore the node. + */ + param_t param = param_find(node->name); + + if (param == PARAM_INVALID) { + debug("ignoring unrecognised parameter '%s'", node->name); + return 1; + } + + /* + * Handle setting the parameter from the node + */ + + switch (node->type) { + case BSON_INT32: + if (param_type(param) != PARAM_TYPE_INT32) { + debug("unexpected type for '%s", node->name); + goto out; + } + + i = node->i; + v = &i; + break; + + case BSON_DOUBLE: + if (param_type(param) != PARAM_TYPE_FLOAT) { + debug("unexpected type for '%s", node->name); + goto out; + } + + f = node->d; + v = &f; + break; + + case BSON_BINDATA: + if (node->subtype != BSON_BIN_BINARY) { + debug("unexpected subtype for '%s", node->name); + goto out; + } + + if (bson_decoder_data_pending(decoder) != param_size(param)) { + debug("bad size for '%s'", node->name); + goto out; + } + + /* XXX check actual file data size? */ + tmp = malloc(param_size(param)); + + if (tmp == NULL) { + debug("failed allocating for '%s'", node->name); + goto out; + } + + if (bson_decoder_copy_data(decoder, tmp)) { + debug("failed copying data for '%s'", node->name); + goto out; + } + + v = tmp; + break; + + default: + debug("unrecognised node type"); + goto out; + } + + if (param_set_internal(param, v, state->mark_saved)) { + debug("error setting value for '%s'", node->name); + goto out; + } + + if (tmp != NULL) { + free(tmp); + tmp = NULL; + } + + /* don't return zero, that means EOF */ + result = 1; + +out: + + if (tmp != NULL) + free(tmp); + + return result; +} + +static int +param_import_internal(int fd, bool mark_saved) +{ + struct bson_decoder_s decoder; + int result = -1; + struct param_import_state state; + + if (bson_decoder_init_file(&decoder, fd, param_import_callback, &state)) { + debug("decoder init failed"); + goto out; + } + + state.mark_saved = mark_saved; + + do { + result = bson_decoder_next(&decoder); + + } while (result > 0); + +out: + + if (result < 0) + debug("BSON error decoding parameters"); + + return result; +} + +int +param_import(int fd) +{ + return param_import_internal(fd, false); +} + +int +param_load(int fd) +{ + param_reset_all(); + return param_import_internal(fd, true); +} + +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); + } +} |