int param_get(param_t param, void *val) { int result = -1; const void *v = param_get_value_ptr(param); if (val != NULL) { memcpy(val, v, param_size(param)); result = 0; } return result; }
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; }
static void do_show_print(void *arg, param_t param) { int32_t i; float f; const char *search_string = (const char*)arg; /* print nothing if search string valid and not matching */ if (arg != NULL && (strcmp(search_string, param_name(param) != 0))) return; printf("%c %s: ", param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), param_name(param)); /* * This case can be expanded to handle printing common structure types. */ switch (param_type(param)) { case PARAM_TYPE_INT32: if (!param_get(param, &i)) { printf("%d\n", i); return; } break; case PARAM_TYPE_FLOAT: if (!param_get(param, &f)) { printf("%4.4f\n", (double)f); return; } break; case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: printf("<struct type %d size %u>\n", 0 + param_type(param), param_size(param)); return; default: printf("<unknown type %d>\n", 0 + param_type(param)); return; } printf("<error fetching parameter %d>\n", param); }
int param_get(param_t param, void *val) { int result = -1; param_lock_reader(); perf_begin(param_get_perf); const void *v = param_get_value_ptr(param); if (val && v) { memcpy(val, v, param_size(param)); result = 0; } perf_end(param_get_perf); param_unlock_reader(); 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) { int result = -1; bool params_changed = false; param_lock_writer(); perf_begin(param_set_perf); if (param_values == NULL) { utarray_new(param_values, ¶m_icd); } if (param_values == NULL) { PX4_ERR("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 }; params_changed = true; /* 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: params_changed = params_changed || s->val.i != *(int32_t *)val; s->val.i = *(int32_t *)val; break; case PARAM_TYPE_FLOAT: params_changed = params_changed || fabsf(s->val.f - * (float *)val) > FLT_EPSILON; s->val.f = *(float *)val; break; case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: if (s->val.p == NULL) { size_t psize = param_size(param); if (psize > 0) { s->val.p = malloc(psize); } else { s->val.p = NULL; } if (s->val.p == NULL) { PX4_ERR("failed to allocate parameter storage"); goto out; } } memcpy(s->val.p, val, param_size(param)); params_changed = true; break; default: goto out; } s->unsaved = !mark_saved; result = 0; if (!mark_saved) { // this is false when importing parameters param_autosave(); } } out: perf_end(param_set_perf); param_unlock_writer(); /* * 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; } #if defined(FLASH_BASED_PARAMS) int param_set_external(param_t param, const void *val, bool mark_saved, bool notify_changes) { return param_set_internal(param, val, mark_saved, notify_changes); } const void *param_get_value_ptr_external(param_t param) { return param_get_value_ptr(param); } #endif 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); }
int param_export(int fd, bool only_unsaved) { perf_begin(param_export_perf); struct param_wbuf_s *s = NULL; int result = -1; struct bson_encoder_s encoder; int shutdown_lock_ret = px4_shutdown_lock(); if (shutdown_lock_ret) { PX4_ERR("px4_shutdown_lock() failed (%i)", shutdown_lock_ret); } // take the file lock do {} while (px4_sem_wait(¶m_sem_save) != 0); param_lock_reader(); uint8_t bson_buffer[256]; bson_encoder_init_buf_file(&encoder, fd, &bson_buffer, sizeof(bson_buffer)); /* 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) { /* * 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; const char *name = param_name(s->param); const size_t size = param_size(s->param); /* append the appropriate BSON type object */ switch (param_type(s->param)) { case PARAM_TYPE_INT32: { const int32_t i = s->val.i; debug("exporting: %s (%d) size: %d val: %d", name, s->param, size, i); if (bson_encoder_append_int(&encoder, name, i)) { PX4_ERR("BSON append failed for '%s'", name); goto out; } } break; case PARAM_TYPE_FLOAT: { const float f = s->val.f; debug("exporting: %s (%d) size: %d val: %.3f", name, s->param, size, (double)f); if (bson_encoder_append_double(&encoder, name, f)) { PX4_ERR("BSON append failed for '%s'", name); goto out; } } break; case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: { const void *value_ptr = param_get_value_ptr(s->param); /* lock as short as possible */ if (bson_encoder_append_binary(&encoder, name, BSON_BIN_BINARY, size, value_ptr)) { PX4_ERR("BSON append failed for '%s'", name); goto out; } } break; default: PX4_ERR("unrecognized parameter type"); goto out; } } result = 0; out: if (result == 0) { if (bson_encoder_fini(&encoder) != PX4_OK) { PX4_ERR("bson encoder finish failed"); } } param_unlock_reader(); px4_sem_post(¶m_sem_save); if (shutdown_lock_ret == 0) { px4_shutdown_unlock(); } perf_end(param_export_perf); return result; }
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); }
static void do_show_print(void *arg, param_t param) { int32_t i; float f; const char *search_string = (const char*)arg; const char *p_name = (const char*)param_name(param); /* print nothing if search string is invalid and not matching */ if (!(arg == NULL)) { /* start search */ char *ss = search_string; char *pp = p_name; bool mismatch = false; /* XXX this comparison is only ok for trailing wildcards */ while (*ss != '\0' && *pp != '\0') { if (*ss == *pp) { ss++; pp++; } else if (*ss == '*') { if (*(ss + 1) != '\0') { warnx("* symbol only allowed at end of search string."); exit(1); } pp++; } else { /* param not found */ return; } } /* the search string must have been consumed */ if (!(*ss == '\0' || *ss == '*')) return; } printf("%c %s: ", param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), param_name(param)); /* * This case can be expanded to handle printing common structure types. */ switch (param_type(param)) { case PARAM_TYPE_INT32: if (!param_get(param, &i)) { printf("%d\n", i); return; } break; case PARAM_TYPE_FLOAT: if (!param_get(param, &f)) { printf("%4.4f\n", (double)f); return; } break; case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: printf("<struct type %d size %u>\n", 0 + param_type(param), param_size(param)); return; default: printf("<unknown type %d>\n", 0 + param_type(param)); return; } printf("<error fetching parameter %d>\n", param); }
static void do_show_print(void *arg, param_t param) { int32_t i; float f; const char *search_string = (const char *)arg; const char *p_name = (const char *)param_name(param); /* print nothing if search string is invalid and not matching */ if (!(arg == NULL)) { /* start search */ const char *ss = search_string; const char *pp = p_name; /* XXX this comparison is only ok for trailing wildcards */ while (*ss != '\0' && *pp != '\0') { // case insensitive comparison (param_name is always upper case) if (toupper(*ss) == *pp) { ss++; pp++; } else if (*ss == '*') { if (*(ss + 1) != '\0') { PX4_WARN("* symbol only allowed at end of search string"); // FIXME - should exit return; } pp++; } else { /* param not found */ return; } } /* the search string must have been consumed */ if (!(*ss == '\0' || *ss == '*') || *pp != '\0') { return; } } PARAM_PRINT("%c %c %s [%d,%d] : ", (param_used(param) ? 'x' : ' '), param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), param_name(param), param_get_used_index(param), param_get_index(param)); /* * This case can be expanded to handle printing common structure types. */ switch (param_type(param)) { case PARAM_TYPE_INT32: if (!param_get(param, &i)) { PARAM_PRINT("%ld\n", (long)i); return; } break; case PARAM_TYPE_FLOAT: if (!param_get(param, &f)) { PARAM_PRINT("%4.4f\n", (double)f); return; } break; case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX: PARAM_PRINT("<struct type %d size %zu>\n", 0 + param_type(param), param_size(param)); return; default: PARAM_PRINT("<unknown type %d>\n", 0 + param_type(param)); return; } PARAM_PRINT("<error fetching parameter %lu>\n", (unsigned long)param); }
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; }