/** lock the parameter store for read access */
static void
param_lock_reader(void)
{
do {} while (px4_sem_wait(&reader_lock_holders_lock) != 0);
++reader_lock_holders;
if (reader_lock_holders == 1) {
// the first reader takes the lock, the next ones are allowed to just continue
do {} while (px4_sem_wait(¶m_sem) != 0);
}
px4_sem_post(&reader_lock_holders_lock);
}
static int
enqueue_work_item_and_wait_for_result(work_q_item_t *item)
{
/* put the work item at the end of the work queue */
lock_queue(&g_work_q);
sq_addlast(&item->link, &(g_work_q.q));
/* Adjust the queue size and potentially the maximum queue size */
if (++g_work_q.size > g_work_q.max_size) {
g_work_q.max_size = g_work_q.size;
}
unlock_queue(&g_work_q);
/* tell the work thread that work is available */
px4_sem_post(&g_work_queued_sema);
/* wait for the result */
px4_sem_wait(&item->wait_sem);
int result = item->result;
destroy_work_item(item);
return result;
}
void
SyslinkMemory::sendAndWait()
{
// TODO: Force the syslink thread to wake up
_link->_queue.force(&msgbuf, sizeof(msgbuf));
px4_sem_wait(&_link->memory_sem);
}
int test_dataman(int argc, char *argv[])
{
int i, num_tasks = 4;
char buffer[DM_MAX_DATA_SIZE];
if (argc > 1) {
num_tasks = atoi(argv[1]);
}
sems = (px4_sem_t *)malloc(num_tasks * sizeof(px4_sem_t));
warnx("Running %d tasks", num_tasks);
for (i = 0; i < num_tasks; i++) {
int task;
char a[16];
sprintf(a, "%d", i);
const char *av[2];
av[0] = a;
av[1] = 0;
px4_sem_init(sems + i, 1, 0);
/* sems use case is a signal */
px4_sem_setprotocol(&sems, SEM_PRIO_NONE);
/* start the task */
if ((task = px4_task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, task_main, (void *)av)) <= 0) {
warn("task start failed");
}
}
for (i = 0; i < num_tasks; i++) {
px4_sem_wait(sems + i);
px4_sem_destroy(sems + i);
}
free(sems);
dm_restart(DM_INIT_REASON_IN_FLIGHT);
for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0) {
break;
}
}
if (i >= NUM_MISSIONS_SUPPORTED) {
warnx("Restart in-flight failed");
return -1;
}
dm_restart(DM_INIT_REASON_POWER_ON);
for (i = 0; i < NUM_MISSIONS_SUPPORTED; i++) {
if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0) {
warnx("Restart power-on failed");
return -1;
}
}
return 0;
}
/** unlock the parameter store */
static void
param_unlock_reader(void)
{
do {} while (px4_sem_wait(&reader_lock_holders_lock) != 0);
--reader_lock_holders;
if (reader_lock_holders == 0) {
// the last reader releases the lock
px4_sem_post(¶m_sem);
}
px4_sem_post(&reader_lock_holders_lock);
}
__EXPORT void
dm_lock(dm_item_t item)
{
/* Make sure data manager has been started and is not shutting down */
if ((g_fd < 0) || g_task_should_exit) {
return;
}
if (item >= DM_KEY_NUM_KEYS) {
return;
}
if (g_item_locks[item]) {
px4_sem_wait(g_item_locks[item]);
}
}
static int
start(void)
{
int task;
px4_sem_init(&g_init_sema, 1, 0);
/* start the worker thread with low priority for disk IO */
if ((task = px4_task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT - 10, 1200, task_main, NULL)) <= 0) {
warn("task start failed");
return -1;
}
/* wait for the thread to actually initialize */
px4_sem_wait(&g_init_sema);
px4_sem_destroy(&g_init_sema);
return 0;
}
int UavcanNode::fw_server(eServerAction action)
{
int rv = -EAGAIN;
switch (action) {
case Start:
case Stop:
case CheckFW:
if (_fw_server_action == None) {
_fw_server_action = action;
px4_sem_wait(&_server_command_sem);
rv = _fw_server_status;
}
break;
default:
rv = -EINVAL;
break;
}
return rv;
}
/** lock the parameter store for write access */
static void
param_lock_writer(void)
{
do {} while (px4_sem_wait(¶m_sem) != 0);
}
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;
}
void lock() { do {} while (px4_sem_wait(&_lock) != 0); }
static int
task_main(int argc, char *argv[])
{
work_q_item_t *work;
/* Initialize global variables */
g_key_offsets[0] = 0;
for (unsigned i = 0; i < (DM_KEY_NUM_KEYS - 1); i++) {
g_key_offsets[i + 1] = g_key_offsets[i] + (g_per_item_max_index[i] * k_sector_size);
}
unsigned max_offset = g_key_offsets[DM_KEY_NUM_KEYS - 1] + (g_per_item_max_index[DM_KEY_NUM_KEYS - 1] * k_sector_size);
for (unsigned i = 0; i < dm_number_of_funcs; i++) {
g_func_counts[i] = 0;
}
/* Initialize the item type locks, for now only DM_KEY_MISSION_STATE supports locking */
px4_sem_init(&g_sys_state_mutex, 1, 1); /* Initially unlocked */
for (unsigned i = 0; i < DM_KEY_NUM_KEYS; i++) {
g_item_locks[i] = NULL;
}
g_item_locks[DM_KEY_MISSION_STATE] = &g_sys_state_mutex;
g_task_should_exit = false;
init_q(&g_work_q);
init_q(&g_free_q);
px4_sem_init(&g_work_queued_sema, 1, 0);
/* See if the data manage file exists and is a multiple of the sector size */
g_task_fd = open(k_data_manager_device_path, O_RDONLY | O_BINARY);
if (g_task_fd >= 0) {
#ifndef __PX4_POSIX
// XXX on Mac OS and Linux the file is not a multiple of the sector sizes
// this might need further inspection.
/* File exists, check its size */
int file_size = lseek(g_task_fd, 0, SEEK_END);
if ((file_size % k_sector_size) != 0) {
PX4_WARN("Incompatible data manager file %s, resetting it", k_data_manager_device_path);
PX4_WARN("Size: %u, sector size: %d", file_size, k_sector_size);
close(g_task_fd);
unlink(k_data_manager_device_path);
}
#else
close(g_task_fd);
#endif
}
/* Open or create the data manager file */
g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY, PX4_O_MODE_666);
if (g_task_fd < 0) {
PX4_WARN("Could not open data manager file %s", k_data_manager_device_path);
px4_sem_post(&g_init_sema); /* Don't want to hang startup */
return -1;
}
if ((unsigned)lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) {
close(g_task_fd);
PX4_WARN("Could not seek data manager file %s", k_data_manager_device_path);
px4_sem_post(&g_init_sema); /* Don't want to hang startup */
return -1;
}
fsync(g_task_fd);
/* see if we need to erase any items based on restart type */
int sys_restart_val;
const char *restart_type_str = "Unkown restart";
if (param_get(param_find("SYS_RESTART_TYPE"), &sys_restart_val) == OK) {
if (sys_restart_val == DM_INIT_REASON_POWER_ON) {
restart_type_str = "Power on restart";
_restart(DM_INIT_REASON_POWER_ON);
} else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) {
restart_type_str = "In flight restart";
_restart(DM_INIT_REASON_IN_FLIGHT);
}
}
/* We use two file descriptors, one for the caller context and one for the worker thread */
/* They are actually the same but we need to some way to reject caller request while the */
/* worker thread is shutting down but still processing requests */
g_fd = g_task_fd;
PX4_INFO("%s, data manager file '%s' size is %d bytes",
restart_type_str, k_data_manager_device_path, max_offset);
/* Tell startup that the worker thread has completed its initialization */
px4_sem_post(&g_init_sema);
/* Start the endless loop, waiting for then processing work requests */
while (true) {
/* do we need to exit ??? */
if ((g_task_should_exit) && (g_fd >= 0)) {
/* Close the file handle to stop further queuing */
g_fd = -1;
}
if (!g_task_should_exit) {
/* wait for work */
px4_sem_wait(&g_work_queued_sema);
}
/* Empty the work queue */
while ((work = dequeue_work_item())) {
/* handle each work item with the appropriate handler */
switch (work->func) {
case dm_write_func:
g_func_counts[dm_write_func]++;
work->result =
_write(work->write_params.item, work->write_params.index, work->write_params.persistence, work->write_params.buf,
work->write_params.count);
break;
case dm_read_func:
g_func_counts[dm_read_func]++;
work->result =
_read(work->read_params.item, work->read_params.index, work->read_params.buf, work->read_params.count);
break;
case dm_clear_func:
g_func_counts[dm_clear_func]++;
work->result = _clear(work->clear_params.item);
break;
case dm_restart_func:
g_func_counts[dm_restart_func]++;
work->result = _restart(work->restart_params.reason);
break;
default: /* should never happen */
work->result = -1;
break;
}
/* Inform the caller that work is done */
px4_sem_post(&work->wait_sem);
}
/* time to go???? */
if ((g_task_should_exit) && (g_fd < 0)) {
break;
}
}
close(g_task_fd);
g_task_fd = -1;
/* The work queue is now empty, empty the free queue */
for (;;) {
if ((work = (work_q_item_t *)sq_remfirst(&(g_free_q.q))) == NULL) {
break;
}
if (work->first) {
free(work);
}
}
destroy_q(&g_work_q);
destroy_q(&g_free_q);
px4_sem_destroy(&g_work_queued_sema);
px4_sem_destroy(&g_sys_state_mutex);
return 0;
}
static inline void
lock_queue(work_q_t *q)
{
px4_sem_wait(&(q->mutex)); /* Acquire the queue lock */
}
int test_dataman(int argc, char *argv[])
{
int i = 0;
unsigned num_tasks = 4;
char buffer[DM_MAX_DATA_SIZE];
if (argc > 1) {
num_tasks = atoi(argv[1]);
}
sems = (px4_sem_t *)malloc(num_tasks * sizeof(px4_sem_t));
task_returned_error = (bool *)calloc(num_tasks, sizeof(bool));
PX4_INFO("Running %d tasks", num_tasks);
for (i = 0; i < num_tasks; i++) {
int task;
char a[16];
snprintf(a, 16, "%d", i);
char *av[] = {"tests_dataman", a, NULL};
px4_sem_init(sems + i, 1, 0);
/* sems use case is a signal */
px4_sem_setprotocol(sems, SEM_PRIO_NONE);
/* start the task */
if ((task = px4_task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, task_main, av)) <= 0) {
PX4_ERR("task start failed");
}
}
for (i = 0; i < num_tasks; i++) {
px4_sem_wait(sems + i);
px4_sem_destroy(sems + i);
}
free(sems);
bool got_error = false;
for (i = 0; i < num_tasks; i++) {
if (task_returned_error[i]) {
got_error = true;
break;
}
}
free(task_returned_error);
if (got_error) {
return -1;
}
dm_restart(DM_INIT_REASON_IN_FLIGHT);
for (i = 0; i < NUM_MISSIONS_TEST; i++) {
if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0) {
break;
}
}
if (i >= NUM_MISSIONS_TEST) {
PX4_ERR("Restart in-flight failed");
return -1;
}
dm_restart(DM_INIT_REASON_POWER_ON);
for (i = 0; i < NUM_MISSIONS_TEST; i++) {
if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD_1, i, buffer, sizeof(buffer)) != 0) {
PX4_ERR("Restart power-on failed");
return -1;
}
}
return 0;
}
static void hrt_lock(void)
{
px4_sem_wait(&_hrt_lock);
}