From 72501df88ed9d537dd3b6eb0941cd19e37ac18d1 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Thu, 18 May 2017 11:59:53 +0200
Subject: [PATCH] dataman: add dm_trylock & add lock for FENCE_POINTS items

---
 src/modules/dataman/dataman.cpp | 50 ++++++++++++++++++++++++++-------
 src/modules/dataman/dataman.h   | 24 ++++++++++++----
 2 files changed, 59 insertions(+), 15 deletions(-)

diff --git a/src/modules/dataman/dataman.cpp b/src/modules/dataman/dataman.cpp
index 0013f351cd..899deba8d5 100644
--- a/src/modules/dataman/dataman.cpp
+++ b/src/modules/dataman/dataman.cpp
@@ -242,7 +242,8 @@ static unsigned int g_key_offsets[DM_KEY_NUM_KEYS];
 
 /* Item type lock mutexes */
 static px4_sem_t *g_item_locks[DM_KEY_NUM_KEYS];
-static px4_sem_t g_sys_state_mutex;
+static px4_sem_t g_sys_state_mutex_mission;
+static px4_sem_t g_sys_state_mutex_fence;
 
 /* The data manager store file handle and file name */
 #if defined(__PX4_POSIX_EAGLE) || defined(__PX4_POSIX_EXCELSIOR)
@@ -1154,22 +1155,48 @@ dm_clear(dm_item_t item)
 	return enqueue_work_item_and_wait_for_result(work);
 }
 
-/** Lock a data Item */
-__EXPORT void
+__EXPORT int
 dm_lock(dm_item_t item)
 {
 	/* Make sure data manager has been started and is not shutting down */
 	if (!is_running() || g_task_should_exit) {
-		return;
+		errno = EINVAL;
+		return -1;
 	}
 
 	if (item >= DM_KEY_NUM_KEYS) {
-		return;
+		errno = EINVAL;
+		return -1;
 	}
 
 	if (g_item_locks[item]) {
-		px4_sem_wait(g_item_locks[item]);
+		return px4_sem_wait(g_item_locks[item]);
 	}
+
+	errno = EINVAL;
+	return -1;
+}
+
+__EXPORT int
+dm_trylock(dm_item_t item)
+{
+	/* Make sure data manager has been started and is not shutting down */
+	if (!is_running() || g_task_should_exit) {
+		errno = EINVAL;
+		return -1;
+	}
+
+	if (item >= DM_KEY_NUM_KEYS) {
+		errno = EINVAL;
+		return -1;
+	}
+
+	if (g_item_locks[item]) {
+		return px4_sem_trywait(g_item_locks[item]);
+	}
+
+	errno = EINVAL;
+	return -1;
 }
 
 /** Unlock a data Item */
@@ -1263,14 +1290,16 @@ task_main(int argc, char *argv[])
 		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 */
+	/* Initialize the item type locks, for now only DM_KEY_MISSION_STATE & DM_KEY_FENCE_POINTS supports locking */
+	px4_sem_init(&g_sys_state_mutex_mission, 1, 1); /* Initially unlocked */
+	px4_sem_init(&g_sys_state_mutex_fence, 1, 1); /* Initially unlocked */
 
 	for (unsigned i = 0; i < DM_KEY_NUM_KEYS; i++) {
 		g_item_locks[i] = nullptr;
 	}
 
-	g_item_locks[DM_KEY_MISSION_STATE] = &g_sys_state_mutex;
+	g_item_locks[DM_KEY_MISSION_STATE] = &g_sys_state_mutex_mission;
+	g_item_locks[DM_KEY_FENCE_POINTS] = &g_sys_state_mutex_fence;
 
 	g_task_should_exit = false;
 
@@ -1406,7 +1435,8 @@ end:
 	destroy_q(&g_work_q);
 	destroy_q(&g_free_q);
 	px4_sem_destroy(&g_work_queued_sema);
-	px4_sem_destroy(&g_sys_state_mutex);
+	px4_sem_destroy(&g_sys_state_mutex_mission);
+	px4_sem_destroy(&g_sys_state_mutex_fence);
 
 	return 0;
 }
diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h
index d33d2a49e6..8f50f48891 100644
--- a/src/modules/dataman/dataman.h
+++ b/src/modules/dataman/dataman.h
@@ -127,16 +127,30 @@ dm_write(
 	size_t buflen			/* Length in bytes of data to retrieve */
 );
 
-/** Lock all items of this type */
-__EXPORT void
+/**
+ * Lock all items of a type. Can be used for atomic updates of multiple items (single items are always updated
+ * atomically).
+ * Note that this lock is independent from dm_read & dm_write calls.
+ * @return 0 on success and lock taken, -1 on error (lock not taken, errno set)
+ */
+__EXPORT int
 dm_lock(
-	dm_item_t item			/* The item type to clear */
+	dm_item_t item			/* The item type to lock */
 );
 
-/** Unlock all items of this type */
+/**
+ * Try to lock all items of a type (@see sem_trywait()).
+ * @return 0 if lock is taken, -1 otherwise (on error or if already locked. errno is set accordingly)
+ */
+__EXPORT int
+dm_trylock(
+	dm_item_t item			/* The item type to lock */
+);
+
+/** Unlock all items of a type */
 __EXPORT void
 dm_unlock(
-	dm_item_t item			/* The item type to clear */
+	dm_item_t item			/* The item type to unlock */
 );
 
 /** Erase all items of this type */