aboutsummaryrefslogtreecommitdiff
path: root/src/modules/dataman/dataman.c
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/dataman/dataman.c')
-rw-r--r--src/modules/dataman/dataman.c107
1 files changed, 84 insertions, 23 deletions
diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c
index 7505ba358..ca1fe9bbb 100644
--- a/src/modules/dataman/dataman.c
+++ b/src/modules/dataman/dataman.c
@@ -1,10 +1,6 @@
/****************************************************************************
*
- * Copyright (c) 2013 PX4 Development Team. All rights reserved.
- * Author: Jean Cyr
- * Lorenz Meier
- * Julian Oes
- * Thomas Gubler
+ * Copyright (c) 2013, 2014 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
@@ -37,6 +33,11 @@
/**
* @file dataman.c
* DATAMANAGER driver.
+ *
+ * @author Jean Cyr
+ * @author Lorenz Meier
+ * @author Julian Oes
+ * @author Thomas Gubler
*/
#include <nuttx/config.h>
@@ -49,6 +50,7 @@
#include <string.h>
#include "dataman.h"
+#include <systemlib/param/param.h>
/**
* data manager app start / stop handling function
@@ -60,9 +62,11 @@ __EXPORT int dataman_main(int argc, char *argv[]);
__EXPORT ssize_t dm_read(dm_item_t item, unsigned char index, void *buffer, size_t buflen);
__EXPORT ssize_t dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const void *buffer, size_t buflen);
__EXPORT int dm_clear(dm_item_t item);
+__EXPORT void dm_lock(dm_item_t item);
+__EXPORT void dm_unlock(dm_item_t item);
__EXPORT int dm_restart(dm_reset_reason restart_type);
-/* Types of function calls supported by the worker task */
+/** Types of function calls supported by the worker task */
typedef enum {
dm_write_func = 0,
dm_read_func,
@@ -71,11 +75,12 @@ typedef enum {
dm_number_of_funcs
} dm_function_t;
-/* Work task work item */
+/** Work task work item */
typedef struct {
sq_entry_t link; /**< list linkage */
sem_t wait_sem;
- dm_function_t func;
+ unsigned char first;
+ unsigned char func;
ssize_t result;
union {
struct {
@@ -100,6 +105,8 @@ typedef struct {
};
} work_q_item_t;
+const size_t k_work_item_allocation_chunk_size = 8;
+
/* Usage statistics */
static unsigned g_func_counts[dm_number_of_funcs];
@@ -109,12 +116,17 @@ static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = {
DM_KEY_FENCE_POINTS_MAX,
DM_KEY_WAYPOINTS_OFFBOARD_0_MAX,
DM_KEY_WAYPOINTS_OFFBOARD_1_MAX,
- DM_KEY_WAYPOINTS_ONBOARD_MAX
+ DM_KEY_WAYPOINTS_ONBOARD_MAX,
+ DM_KEY_MISSION_STATE_MAX
};
/* Table of offset for index 0 of each item type */
static unsigned int g_key_offsets[DM_KEY_NUM_KEYS];
+/* Item type lock mutexes */
+static sem_t *g_item_locks[DM_KEY_NUM_KEYS];
+static sem_t g_sys_state_mutex;
+
/* The data manager store file handle and file name */
static int g_fd = -1, g_task_fd = -1;
static const char *k_data_manager_device_path = "/fs/microsd/dataman";
@@ -134,22 +146,22 @@ static work_q_t g_work_q; /* pending work items. To be consumed by worker thread
sem_t g_work_queued_sema; /* To notify worker thread a work item has been queued */
sem_t g_init_sema;
-static bool g_task_should_exit; /**< if true, dataman task should exit */
+static bool g_task_should_exit; /**< if true, dataman task should exit */
#define DM_SECTOR_HDR_SIZE 4 /* data manager per item header overhead */
static const unsigned k_sector_size = DM_MAX_DATA_SIZE + DM_SECTOR_HDR_SIZE; /* total item sorage space */
static void init_q(work_q_t *q)
{
- sq_init(&(q->q)); /* Initialize the NuttX queue structure */
+ sq_init(&(q->q)); /* Initialize the NuttX queue structure */
sem_init(&(q->mutex), 1, 1); /* Queue is initially unlocked */
- q->size = q->max_size = 0; /* Queue is initially empty */
+ q->size = q->max_size = 0; /* Queue is initially empty */
}
static inline void
destroy_q(work_q_t *q)
{
- sem_destroy(&(q->mutex)); /* Destroy the queue lock */
+ sem_destroy(&(q->mutex)); /* Destroy the queue lock */
}
static inline void
@@ -177,9 +189,23 @@ create_work_item(void)
unlock_queue(&g_free_q);
- /* If we there weren't any free items then obtain memory for a new one */
- if (item == NULL)
- item = (work_q_item_t *)malloc(sizeof(work_q_item_t));
+ /* If we there weren't any free items then obtain memory for a new ones */
+ if (item == NULL) {
+ item = (work_q_item_t *)malloc(k_work_item_allocation_chunk_size * sizeof(work_q_item_t));
+ if (item) {
+ item->first = 1;
+ lock_queue(&g_free_q);
+ for (size_t i = 1; i < k_work_item_allocation_chunk_size; i++) {
+ (item + i)->first = 0;
+ sq_addfirst(&(item + i)->link, &(g_free_q.q));
+ }
+ /* Update the queue size and potentially the maximum queue size */
+ g_free_q.size += k_work_item_allocation_chunk_size - 1;
+ if (g_free_q.size > g_free_q.max_size)
+ g_free_q.max_size = g_free_q.size;
+ unlock_queue(&g_free_q);
+ }
+ }
/* If we got one then lock the item*/
if (item)
@@ -299,7 +325,9 @@ _write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const v
buffer[1] = persistence;
buffer[2] = 0;
buffer[3] = 0;
- memcpy(buffer + DM_SECTOR_HDR_SIZE, buf, count);
+ if (count > 0) {
+ memcpy(buffer + DM_SECTOR_HDR_SIZE, buf, count);
+ }
count += DM_SECTOR_HDR_SIZE;
len = -1;
@@ -411,7 +439,7 @@ _clear(dm_item_t item)
return result;
}
-/* Tell the data manager about the type of the last reset */
+/** Tell the data manager about the type of the last reset */
static int
_restart(dm_reset_reason reason)
{
@@ -480,7 +508,7 @@ _restart(dm_reset_reason reason)
return result;
}
-/* write to the data manager file */
+/** Write to the data manager file */
__EXPORT ssize_t
dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const void *buf, size_t count)
{
@@ -505,7 +533,7 @@ dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const
return (ssize_t)enqueue_work_item_and_wait_for_result(work);
}
-/* Retrieve from the data manager file */
+/** Retrieve from the data manager file */
__EXPORT ssize_t
dm_read(dm_item_t item, unsigned char index, void *buf, size_t count)
{
@@ -549,6 +577,32 @@ dm_clear(dm_item_t item)
return enqueue_work_item_and_wait_for_result(work);
}
+__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]) {
+ sem_wait(g_item_locks[item]);
+ }
+}
+
+__EXPORT void
+dm_unlock(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]) {
+ sem_post(g_item_locks[item]);
+ }
+}
+
/* Tell the data manager about the type of the last reset */
__EXPORT int
dm_restart(dm_reset_reason reason)
@@ -589,6 +643,12 @@ task_main(int argc, char *argv[])
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 */
+ 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);
@@ -717,13 +777,14 @@ task_main(int argc, char *argv[])
for (;;) {
if ((work = (work_q_item_t *)sq_remfirst(&(g_free_q.q))) == NULL)
break;
-
- free(work);
+ if (work->first)
+ free(work);
}
destroy_q(&g_work_q);
destroy_q(&g_free_q);
sem_destroy(&g_work_queued_sema);
+ sem_destroy(&g_sys_state_mutex);
return 0;
}
@@ -736,7 +797,7 @@ start(void)
sem_init(&g_init_sema, 1, 0);
/* start the worker thread */
- if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, task_main, NULL)) <= 0) {
+ if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, task_main, NULL)) <= 0) {
warn("task start failed");
return -1;
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-01 02:35:11 +0000