Create system state entry in dataman - ATTN Anton

Create persistent system state id for data manager to store system state
that will persist across resets.

2 files changed, 70 insertions, 6 deletions

diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c
index 1a65313e8..96dc98f2a 100644
--- a/src/modules/dataman/dataman.c
+++ b/src/modules/dataman/dataman.c
@@ -61,6 +61,8 @@ __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 */
@@ -113,12 +115,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_SYSTEM_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";
@@ -138,22 +145,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
@@ -567,6 +574,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)
@@ -607,6 +640,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_SYSTEM_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_SYSTEM_STATE] = &g_sys_state_mutex;
+
 	g_task_should_exit = false;
 
 	init_q(&g_work_q);
@@ -742,6 +781,7 @@ task_main(int argc, char *argv[])
 	destroy_q(&g_work_q);
 	destroy_q(&g_free_q);
 	sem_destroy(&g_work_queued_sema);
+	sem_destroy(&g_sys_state_mutex);
 
 	return 0;
 }
diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h
index 1dfb26f73..f2b6cd4d3 100644
--- a/src/modules/dataman/dataman.h
+++ b/src/modules/dataman/dataman.h
@@ -53,6 +53,7 @@ extern "C" {
 		DM_KEY_WAYPOINTS_OFFBOARD_0,	/* Mission way point coordinates sent over mavlink */
 		DM_KEY_WAYPOINTS_OFFBOARD_1,	/* (alernate between 0 and 1) */
 		DM_KEY_WAYPOINTS_ONBOARD,	/* Mission way point coordinates generated onboard */
+		DM_KEY_SYSTEM_STATE,		/* Persistent system state storage */
 		DM_KEY_NUM_KEYS			/* Total number of item types defined */
 	} dm_item_t;
 
@@ -62,7 +63,8 @@ extern "C" {
 		DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES,
 		DM_KEY_WAYPOINTS_OFFBOARD_0_MAX = NUM_MISSIONS_SUPPORTED,
 		DM_KEY_WAYPOINTS_OFFBOARD_1_MAX = NUM_MISSIONS_SUPPORTED,
-		DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED
+		DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED,
+		DM_KEY_SYSTEM_STATE_MAX = 1
 	};
 
 	/** Data persistence levels */
@@ -101,6 +103,18 @@ extern "C" {
 		size_t buflen			/* Length in bytes of data to retrieve */
 	);
 
+	/** Lock all items of this type */
+	__EXPORT void
+	dm_lock(
+		dm_item_t item			/* The item type to clear */
+		);
+
+	/** Unlock all items of this type */
+	__EXPORT void
+	dm_unlock(
+		dm_item_t item			/* The item type to clear */
+		);
+
 	/** Erase all items of this type */
 	__EXPORT int
 	dm_clear(
@@ -113,6 +127,16 @@ extern "C" {
 		dm_reset_reason restart_type	/* The last reset type */
 	);
 
+	/* NOTE: The following structure defines the persistent system state data stored in the single
+	   entry DM_KEY_SYSTEM_STATE_KEY item type. It contains global system state information that
+	   needs to survive restarts. This definition is application specific so it doesn't really belong
+	   in this header, but till I find it a better home here it is */
+
+	typedef struct {
+		char current_offboard_waypoint_id;	/* the index of the active offboard waypoint data */
+							/* (DM_KEY_WAYPOINTS_OFFBOARD_n) or -1 for none */
+	} persistent_system_state_t;
+
 #ifdef __cplusplus
 }
 #endif