modules: dataman: Move backend specific out of main routines

src/modules/dataman/dataman.c

@@ -81,6 +81,8 @@ static ssize_t _file_write(dm_item_t item, unsigned index, dm_persitence_t persi
 static ssize_t _file_read(dm_item_t item, unsigned index, void *buf, size_t count);
 static int  _file_clear(dm_item_t item);
 static int  _file_restart(dm_reset_reason reason);
+static int _file_initialize(unsigned max_offset);
+static void _file_shutdown(void);
 
 /* Private Ram based Operations */
 static ssize_t _ram_write(dm_item_t item, unsigned index, dm_persitence_t persistence, const void *buf,
@@ -88,12 +90,16 @@ static ssize_t _ram_write(dm_item_t item, unsigned index, dm_persitence_t persis
 static ssize_t _ram_read(dm_item_t item, unsigned index, void *buf, size_t count);
 static int  _ram_clear(dm_item_t item);
 static int  _ram_restart(dm_reset_reason reason);
+static int _ram_initialize(unsigned max_offset);
+static void _ram_shutdown(void);
 
 typedef struct dm_operations_t {
 	ssize_t (*write)(dm_item_t item, unsigned index, dm_persitence_t persistence, const void *buf, size_t count);
 	ssize_t (*read)(dm_item_t item, unsigned index, void *buf, size_t count);
 	int (*clear)(dm_item_t item);
 	int (*restart)(dm_reset_reason reason);
+	int (*initialize)(unsigned max_offset);
+	void (*shutdown)(void);
 } dm_operations_t;
 
 static dm_operations_t dm_file_operations = {
@@ -101,6 +107,8 @@ static dm_operations_t dm_file_operations = {
 	.read    = _file_read,
 	.clear   = _file_clear,
 	.restart = _file_restart,
+	.initialize = _file_initialize,
+	.shutdown = _file_shutdown,
 };
 
 static dm_operations_t dm_ram_operations = {
@@ -108,9 +116,11 @@ static dm_operations_t dm_ram_operations = {
 	.read    = _ram_read,
 	.clear   = _ram_clear,
 	.restart = _ram_restart,
+	.initialize = _ram_initialize,
+	.shutdown = _ram_shutdown,
 };
 
-static dm_operations_t *g_dm_ops = &dm_file_operations;
+static dm_operations_t *g_dm_ops;
 
 static struct {
 	union {
@@ -195,6 +205,13 @@ static const char *default_device_path = PX4_ROOTFSDIR"/fs/microsd/dataman";
 #endif
 static char *k_data_manager_device_path = NULL;
 
+static enum {
+	BACKEND_NONE = 0,
+	BACKEND_FILE,
+	BACKEND_RAM,
+	BACKEND_LAST
+} backend = BACKEND_NONE;
+
 /* The data manager work queues */
 
 typedef struct {
@@ -762,6 +779,96 @@ _file_restart(dm_reset_reason reason)
 	return result;
 }
 
+static int
+_file_initialize(unsigned max_offset)
+{
+	/* See if the data manage file exists and is a multiple of the sector size */
+	dm_operations_data.file.fd = open(k_data_manager_device_path, O_RDONLY | O_BINARY);
+
+	if (dm_operations_data.file.fd >= 0) {
+		// Read the mission state and check the hash
+		struct dataman_compat_s compat_state;
+		int ret = g_dm_ops->read(DM_KEY_COMPAT, 0, &compat_state, sizeof(compat_state));
+
+		bool incompat = true;
+
+		if (ret == sizeof(compat_state)) {
+			if (compat_state.key == DM_COMPAT_KEY) {
+				incompat = false;
+			}
+		}
+
+		close(dm_operations_data.file.fd);
+
+		if (incompat) {
+			unlink(k_data_manager_device_path);
+		}
+	}
+
+	/* Open or create the data manager file */
+	dm_operations_data.file.fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY, PX4_O_MODE_666);
+
+	if (dm_operations_data.file.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(dm_operations_data.file.fd, max_offset, SEEK_SET) != max_offset) {
+		close(dm_operations_data.file.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;
+	}
+
+	/* Write current compat info */
+	struct dataman_compat_s compat_state;
+	compat_state.key = DM_COMPAT_KEY;
+	int ret = g_dm_ops->write(DM_KEY_COMPAT, 0, DM_PERSIST_POWER_ON_RESET, &compat_state, sizeof(compat_state));
+
+	if (ret != sizeof(compat_state)) {
+		PX4_ERR("Failed writing compat: %d", ret);
+	}
+
+	fsync(dm_operations_data.file.fd);
+	dm_operations_data.running = true;
+
+	return 0;
+}
+
+static int
+_ram_initialize(unsigned max_offset)
+{
+	/* In memory */
+	dm_operations_data.ram.data = malloc(max_offset);
+
+	if (dm_operations_data.ram.data == NULL) {
+		PX4_WARN("Could not allocate %d bytes of memory", max_offset);
+		px4_sem_post(&g_init_sema); /* Don't want to hang startup */
+		return -1;
+	}
+
+	memset(dm_operations_data.ram.data, 0, max_offset);
+	dm_operations_data.ram.data_end = &dm_operations_data.ram.data[max_offset - 1];
+	dm_operations_data.running = true;
+
+	return 0;
+}
+
+static void
+_file_shutdown(void)
+{
+	close(dm_operations_data.file.fd);
+	dm_operations_data.running = false;
+}
+
+static void
+_ram_shutdown(void)
+{
+	free(dm_operations_data.ram.data);
+	dm_operations_data.running = false;
+}
+
 /** Write to the data manager file */
 __EXPORT ssize_t
 dm_write(dm_item_t item, unsigned index, dm_persitence_t persistence, const void *buf, size_t count)
@@ -901,8 +1008,19 @@ static int
 task_main(int argc, char *argv[])
 {
 	/* Dataman can use disk or RAM */
+	switch (backend) {
+	case BACKEND_FILE:
+		g_dm_ops = &dm_file_operations;
+		break;
 
-	const bool on_disk = k_data_manager_device_path != NULL;
+	case BACKEND_RAM:
+		g_dm_ops = &dm_ram_operations;
+		break;
+
+	default:
+		PX4_WARN("No valid backend set.");
+		return -1;
+	}
 
 	work_q_item_t *work;
 
@@ -939,75 +1057,13 @@ task_main(int argc, char *argv[])
 
 	px4_sem_setprotocol(&g_work_queued_sema, SEM_PRIO_NONE);
 
-	if (!on_disk) {
+	int ret = g_dm_ops->initialize(max_offset);
 
-		/* In memory */
+	if (ret) {
-		dm_operations_data.ram.data = malloc(max_offset);
+		g_task_should_exit = true;
-
+		goto end;
-		if (dm_operations_data.ram.data == NULL) {
-			PX4_WARN("Could not allocate %d bytes of memory", max_offset);
-			px4_sem_post(&g_init_sema); /* Don't want to hang startup */
-			return -1;
-		}
-
-		memset(dm_operations_data.ram.data, 0, max_offset);
-		dm_operations_data.ram.data_end = &dm_operations_data.ram.data[max_offset - 1];
-
-	} else {
-		/* See if the data manage file exists and is a multiple of the sector size */
-		dm_operations_data.file.fd = open(k_data_manager_device_path, O_RDONLY | O_BINARY);
-
-		if (dm_operations_data.file.fd >= 0) {
-			// Read the mission state and check the hash
-			struct dataman_compat_s compat_state;
-			int ret = g_dm_ops->read(DM_KEY_COMPAT, 0, &compat_state, sizeof(compat_state));
-
-			bool incompat = true;
-
-			if (ret == sizeof(compat_state)) {
-				if (compat_state.key == DM_COMPAT_KEY) {
-					incompat = false;
-				}
-			}
-
-			close(dm_operations_data.file.fd);
-
-			if (incompat) {
-				unlink(k_data_manager_device_path);
-			}
-		}
-
-		/* Open or create the data manager file */
-		dm_operations_data.file.fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY, PX4_O_MODE_666);
-
-		if (dm_operations_data.file.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(dm_operations_data.file.fd, max_offset, SEEK_SET) != max_offset) {
-			close(dm_operations_data.file.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;
-		}
-
-		/* Write current compat info */
-		struct dataman_compat_s compat_state;
-		compat_state.key = DM_COMPAT_KEY;
-		int ret = g_dm_ops->write(DM_KEY_COMPAT, 0, DM_PERSIST_POWER_ON_RESET, &compat_state, sizeof(compat_state));
-
-		if (ret != sizeof(compat_state)) {
-			PX4_ERR("Failed writing compat: %d", ret);
-		}
-
-		fsync(dm_operations_data.file.fd);
 	}
 
-	g_dm_ops = on_disk ? &dm_file_operations : &dm_ram_operations;
-	dm_operations_data.running = true;
-
 	/* see if we need to erase any items based on restart type */
 	int sys_restart_val;
 
@@ -1024,11 +1080,11 @@ task_main(int argc, char *argv[])
 		}
 	}
 
-	if (on_disk && sys_restart_val != DM_INIT_REASON_POWER_ON) {
+	if (backend == BACKEND_FILE && sys_restart_val != DM_INIT_REASON_POWER_ON) {
 		PX4_INFO("%s, data manager file '%s' size is %d bytes",
 			 restart_type_str, k_data_manager_device_path, max_offset);
 
-	} else if (!on_disk) {
+	} else if (backend == BACKEND_RAM) {
 		PX4_INFO("%s, data manager RAM size is %d bytes",
 			 restart_type_str, max_offset);
 	}
@@ -1089,14 +1145,7 @@ task_main(int argc, char *argv[])
 		}
 	}
 
-	dm_operations_data.running = false;
+	g_dm_ops->shutdown();
-
-	if (on_disk) {
-		close(dm_operations_data.file.fd);
-
-	} else {
-		free(dm_operations_data.ram.data);
-	}
 
 	/* The work queue is now empty, empty the free queue */
 	for (;;) {
@@ -1109,6 +1158,8 @@ task_main(int argc, char *argv[])
 		}
 	}
 
+end:
+	backend = BACKEND_NONE;
 	destroy_q(&g_work_q);
 	destroy_q(&g_free_q);
 	px4_sem_destroy(&g_work_queued_sema);
@@ -1169,8 +1220,6 @@ usage(void)
 int
 dataman_main(int argc, char *argv[])
 {
-	bool in_ram = false;
-
 	if (argc < 2) {
 		usage();
 		return -1;
@@ -1192,12 +1241,25 @@ dataman_main(int argc, char *argv[])
 		while ((ch = px4_getopt(argc, argv, "f:r", &dmoptind, &dmoptarg)) != EOF) {
 			switch (ch) {
 			case 'f':
+				if (backend != BACKEND_NONE) {
+					PX4_WARN("-f and -r are mutually exclusive");
+					usage();
+					return -1;
+				}
+
+				backend = BACKEND_FILE;
 				k_data_manager_device_path = strdup(dmoptarg);
 				PX4_INFO("dataman file set to: %s", k_data_manager_device_path);
 				break;
 
 			case 'r':
-				in_ram = true;
+				if (backend != BACKEND_NONE) {
+					PX4_WARN("-f and -r are mutually exclusive");
+					usage();
+					return -1;
+				}
+
+				backend = BACKEND_RAM;
 				break;
 
 
@@ -1208,13 +1270,8 @@ dataman_main(int argc, char *argv[])
 			}
 		}
 
-		if (k_data_manager_device_path != NULL && in_ram) {
+		if (backend == BACKEND_NONE) {
-			PX4_WARN("-f and -r are mutually exclusive");
+			backend = BACKEND_FILE;
-			usage();
-			return -1;
-		}
-
-		if (k_data_manager_device_path == NULL && !in_ram) {
 			k_data_manager_device_path = strdup(default_device_path);
 		}