void
param_reset_excludes(const char *excludes[], int num_excludes)
{
param_lock();
param_t param;
for (param = 0; handle_in_range(param); param++) {
const char *name = param_name(param);
bool exclude = false;
for (int index = 0; index < num_excludes; index ++) {
int len = strlen(excludes[index]);
if ((excludes[index][len - 1] == '*'
&& strncmp(name, excludes[index], len - 1) == 0)
|| strcmp(name, excludes[index]) == 0) {
exclude = true;
break;
}
}
if (!exclude) {
param_reset(param);
}
}
param_unlock();
param_notify_changes();
}
int
param_reset(param_t param)
{
struct param_wbuf_s *s = NULL;
bool param_found = false;
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_found = true;
}
param_unlock();
if (s != NULL) {
param_notify_changes();
}
return (!param_found);
}
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();
}
int
param_get(param_t param, void *val)
{
int result = -1;
param_lock();
if (!handle_in_range(param)) {
return result;
}
union param_value_u value;
if (update_from_shmem(param, &value)) {
set_called_from_get = 1;
param_set_internal(param, &value, true, false, false);
set_called_from_get = 0;
}
const void *v = param_get_value_ptr(param);
if (val != NULL) {
memcpy(val, v, param_size(param));
result = 0;
}
#ifdef ENABLE_SHMEM_DEBUG
if (param_type(param) == PARAM_TYPE_INT32) {
PX4_INFO("param_get for %s : %d\n", param_name(param), ((union param_value_u *)val)->i);
}
else if (param_type(param) == PARAM_TYPE_FLOAT) {
PX4_INFO("param_get for %s : %f\n", param_name(param), (double)((union param_value_u *)val)->f);
}
else {
PX4_INFO("Unknown param type for %s\n", param_name(param));
}
#endif
param_unlock();
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;
}
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;
}
static int
param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_changes)
{
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 && notify_changes) {
param_notify_changes();
}
return result;
}
int
param_set(param_t param, const void *val)
{
return param_set_internal(param, val, false, true);
}
int
param_set_no_notification(param_t param, const void *val)
{
return param_set_internal(param, val, false, false);
}
bool
param_used(param_t param)
{
int param_index = param_get_index(param);
if (param_index < 0) {
return false;
}
return param_changed_storage[param_index / bits_per_allocation_unit] &
(1 << param_index % bits_per_allocation_unit);
}
static int
param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_changes, bool is_saved)
{
int result = -1;
bool params_changed = false;
PX4_DEBUG("param_set_internal params: param = %d, val = 0x%X, mark_saved: %d, notify_changes: %d",
param, val, (int)mark_saved, (int)notify_changes);
param_lock();
if (!handle_in_range(param)) {
return result;
}
mark_saved = true; //mark all params as saved
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 (!param_import_done) { notify_changes = 0; }
if (params_changed && notify_changes) {
param_notify_changes(is_saved);
}
if (result == 0 && !set_called_from_get) {
update_to_shmem(param, *(union param_value_u *)val);
}
#ifdef ENABLE_SHMEM_DEBUG
if (param_type(param) == PARAM_TYPE_INT32) {
PX4_INFO("param_set for %s : %d\n", param_name(param), ((union param_value_u *)val)->i);
}
else if (param_type(param) == PARAM_TYPE_FLOAT) {
PX4_INFO("param_set for %s : %f\n", param_name(param), (double)((union param_value_u *)val)->f);
}
else {
PX4_INFO("Unknown param type for %s\n", param_name(param));
}
#endif
return result;
}
int
param_set(param_t param, const void *val)
{
return param_set_internal(param, val, false, true, false);
}
int
param_set_no_autosave(param_t param, const void *val)
{
return param_set_internal(param, val, false, true, true);
}
int
param_set_no_notification(param_t param, const void *val)
{
return param_set_internal(param, val, false, false, false);
}
bool
param_used(param_t param)
{
// TODO FIXME: for now all params are used
return true;
int param_index = param_get_index(param);
if (param_index < 0) {
return false;
}
return param_changed_storage[param_index / bits_per_allocation_unit] &
(1 << param_index % bits_per_allocation_unit);
}
void param_set_used_internal(param_t param)
{
int param_index = param_get_index(param);
if (param_index < 0) {
return;
}
param_changed_storage[param_index / bits_per_allocation_unit] |=
(1 << param_index % bits_per_allocation_unit);
}
int
param_export(int fd, bool only_unsaved)
{
struct param_wbuf_s *s = NULL;
struct bson_encoder_s encoder;
int result = -1;
param_lock();
param_bus_lock(true);
bson_encoder_init_file(&encoder, fd);
param_bus_lock(false);
/* 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);
const char *name = param_name(s->param);
/* lock as short as possible */
param_bus_lock(true);
if (bson_encoder_append_int(&encoder, name, i)) {
param_bus_lock(false);
debug("BSON append failed for '%s'", name);
goto out;
}
}
break;
case PARAM_TYPE_FLOAT: {
param_get(s->param, &f);
const char *name = param_name(s->param);
/* lock as short as possible */
param_bus_lock(true);
if (bson_encoder_append_double(&encoder, name, f)) {
param_bus_lock(false);
debug("BSON append failed for '%s'", name);
goto out;
}
}
break;
case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: {
const char *name = param_name(s->param);
const size_t size = param_size(s->param);
const void *value_ptr = param_get_value_ptr(s->param);
/* lock as short as possible */
param_bus_lock(true);
if (bson_encoder_append_binary(&encoder,
name,
BSON_BIN_BINARY,
size,
value_ptr)) {
param_bus_lock(false);
debug("BSON append failed for '%s'", name);
goto out;
}
}
break;
default:
debug("unrecognized parameter type");
goto out;
}
param_bus_lock(false);
/* allow this process to be interrupted by another process / thread */
usleep(5);
}
result = 0;
out:
param_unlock();
if (result == 0) {
result = bson_encoder_fini(&encoder);
}
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 int
param_export_internal(bool only_unsaved)
{
struct param_wbuf_s *s = NULL;
struct bson_encoder_s encoder;
int result = -1;
param_lock();
/* Use realloc */
bson_encoder_init_buf(&encoder, NULL, 0);
/* 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_external(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) {
/* Finalize the bison encoding*/
bson_encoder_fini(&encoder);
/* Get requiered space */
size_t buf_size = bson_encoder_buf_size(&encoder);
/* Get a buffer from the flash driver with enough space */
uint8_t *buffer;
result = parameter_flashfs_alloc(parameters_token, &buffer, &buf_size);
if (result == OK) {
/* Check for a write that has no changes */
uint8_t *was_buffer;
size_t was_buf_size;
int was_result = parameter_flashfs_read(parameters_token, &was_buffer, &was_buf_size);
void *enc_buff = bson_encoder_buf_data(&encoder);
bool commit = was_result < OK || was_buf_size != buf_size || 0 != memcmp(was_buffer, enc_buff, was_buf_size);
if (commit) {
memcpy(buffer, enc_buff, buf_size);
result = parameter_flashfs_write(parameters_token, buffer, buf_size);
result = result == buf_size ? OK : -EFBIG;
}
free(enc_buff);
}
}
return result;
}
