diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index cb27acdd49..239c3b5936 100644 --- a/src/modules/dataman/dataman.c +++ b/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 */ - 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]; - - } 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); + if (ret) { + g_task_should_exit = true; + goto end; } - 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; - - if (on_disk) { - close(dm_operations_data.file.fd); - - } else { - free(dm_operations_data.ram.data); - } + g_dm_ops->shutdown(); /* 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) { - PX4_WARN("-f and -r are mutually exclusive"); - usage(); - return -1; - } - - if (k_data_manager_device_path == NULL && !in_ram) { + if (backend == BACKEND_NONE) { + backend = BACKEND_FILE; k_data_manager_device_path = strdup(default_device_path); }