PX4-Autopilot/src/modules/dataman/dataman.c

/****************************************************************************
 *
 *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
 *   Author: 	Jean Cyr
 *              Lorenz Meier
 *              Julian Oes
 *              Thomas Gubler
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/
/**
 * @file dataman.c
 * DATAMANAGER driver.
 */

#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <fcntl.h>
#include <systemlib/systemlib.h>
#include <systemlib/err.h>
#include <queue.h>
#include <string.h>

#include "dataman.h"

/**
 * data manager app start / stop handling function
 *
 * @ingroup apps
 */

__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 int dm_restart(dm_reset_reason restart_type);

/* Types of function calls supported by the worker task */
typedef enum {
	dm_write_func = 0,
	dm_read_func,
	dm_clear_func,
	dm_restart_func,
	dm_number_of_funcs
} dm_function_t;

/* Work task work item */
typedef struct {
	sq_entry_t link;	/**< list linkage */
	sem_t wait_sem;
	dm_function_t func;
	ssize_t result;
	union {
		struct {
			dm_item_t item;
			unsigned char index;
			dm_persitence_t persistence;
			const void *buf;
			size_t count;
		} write_params;
		struct {
			dm_item_t item;
			unsigned char index;
			void *buf;
			size_t count;
		} read_params;
		struct {
			dm_item_t item;
		} clear_params;
		struct {
			dm_reset_reason reason;
		} restart_params;
	};
} work_q_item_t;

/* Usage statistics */
static unsigned g_func_counts[dm_number_of_funcs];

/* table of maximum number of instances for each item type */
static const unsigned g_per_item_max_index[DM_KEY_NUM_KEYS] = {
	DM_KEY_SAFE_POINTS_MAX,
	DM_KEY_FENCE_POINTS_MAX,
	DM_KEY_WAYPOINTS_OFFBOARD_0_MAX,
	DM_KEY_WAYPOINTS_OFFBOARD_1_MAX,
	DM_KEY_WAYPOINTS_ONBOARD_MAX
};

/* Table of offset for index 0 of each item type */
static unsigned int g_key_offsets[DM_KEY_NUM_KEYS];

/* 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";

/* The data manager work queues */

typedef struct {
	sq_queue_t q;		/* Nuttx queue */
	sem_t mutex;		/* Mutual exclusion on work queue adds and deletes */
	unsigned size;		/* Current size of queue */
	unsigned max_size;	/* Maximum queue size reached */
} work_q_t;

static work_q_t g_free_q;	/* queue of free work items. So that we don't always need to call malloc and free*/
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 */

#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 */
	sem_init(&(q->mutex), 1, 1);	/* Queue is initially unlocked */
	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 */
}

static inline void
lock_queue(work_q_t *q)
{
	sem_wait(&(q->mutex));	/* Acquire the queue lock */
}

static inline void
unlock_queue(work_q_t *q)
{
	sem_post(&(q->mutex));	/* Release the queue lock */
}

static work_q_item_t *
create_work_item(void)
{
	work_q_item_t *item;

	/* Try to reuse item from free item queue */
	lock_queue(&g_free_q);

	if ((item = (work_q_item_t *)sq_remfirst(&(g_free_q.q))))
		g_free_q.size--;

	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 got one then lock the item*/
	if (item)
		sem_init(&item->wait_sem, 1, 0); /* Caller will wait on this... initially locked */

	/* return the item pointer, or NULL if all failed */
	return item;
}

/* Work queue management functions */

static inline void
destroy_work_item(work_q_item_t *item)
{
	sem_destroy(&item->wait_sem); /* Destroy the item lock */
	/* Return the item to the free item queue for later reuse */
	lock_queue(&g_free_q);
	sq_addfirst(&item->link, &(g_free_q.q));

	/* Update the queue size and potentially the maximum queue size */
	if (++g_free_q.size > g_free_q.max_size)
		g_free_q.max_size = g_free_q.size;

	unlock_queue(&g_free_q);
}

static inline work_q_item_t *
dequeue_work_item(void)
{
	work_q_item_t *work;

	/* retrieve the 1st item on the work queue */
	lock_queue(&g_work_q);

	if ((work = (work_q_item_t *)sq_remfirst(&g_work_q.q)))
		g_work_q.size--;

	unlock_queue(&g_work_q);
	return work;
}

static int
enqueue_work_item_and_wait_for_result(work_q_item_t *item)
{
	/* put the work item at the end of the work queue */
	lock_queue(&g_work_q);
	sq_addlast(&item->link, &(g_work_q.q));

	/* Adjust the queue size and potentially the maximum queue size */
	if (++g_work_q.size > g_work_q.max_size)
		g_work_q.max_size = g_work_q.size;

	unlock_queue(&g_work_q);

	/* tell the work thread that work is available */
	sem_post(&g_work_queued_sema);

	/* wait for the result */
	sem_wait(&item->wait_sem);

	int result = item->result;

	destroy_work_item(item);

	return result;
}

/* Calculate the offset in file of specific item */
static int
calculate_offset(dm_item_t item, unsigned char index)
{

	/* Make sure the item type is valid */
	if (item >= DM_KEY_NUM_KEYS)
		return -1;

	/* Make sure the index for this item type is valid */
	if (index >= g_per_item_max_index[item])
		return -1;

	/* Calculate and return the item index based on type and index */
	return g_key_offsets[item] + (index * k_sector_size);
}

/* Each data item is stored as follows
 *
 * byte 0: Length of user data item
 * byte 1: Persistence of this data item
 * byte 2: Unused (for future use)
 * byte 3: Unused (for future use)
 * byte DM_SECTOR_HDR_SIZE... : data item value
 *
 * The total size must not exceed k_sector_size
 */

/* write to the data manager file */
static ssize_t
_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const void *buf, size_t count)
{
	unsigned char buffer[k_sector_size];
	size_t len;
	int offset;

	/* Get the offset for this item */
	offset = calculate_offset(item, index);

	/* If item type or index out of range, return error */
	if (offset < 0)
		return -1;

	/* Make sure caller has not given us more data than we can handle */
	if (count > DM_MAX_DATA_SIZE)
		return -1;

	/* Write out the data, prefixed with length and persistence level */
	buffer[0] = count;
	buffer[1] = persistence;
	buffer[2] = 0;
	buffer[3] = 0;
	memcpy(buffer + DM_SECTOR_HDR_SIZE, buf, count);
	count += DM_SECTOR_HDR_SIZE;

	len = -1;

	/* Seek to the right spot in the data manager file and write the data item */
	if (lseek(g_task_fd, offset, SEEK_SET) == offset)
		if ((len = write(g_task_fd, buffer, count)) == count)
			fsync(g_task_fd); /* Make sure data is written to physical media */

	/* Make sure the write succeeded */
	if (len != count)
		return -1;

	/* All is well... return the number of user data written */
	return count - DM_SECTOR_HDR_SIZE;
}

/* Retrieve from the data manager file */
static ssize_t
_read(dm_item_t item, unsigned char index, void *buf, size_t count)
{
	unsigned char buffer[k_sector_size];
	int len, offset;

	/* Get the offset for this item */
	offset = calculate_offset(item, index);

	/* If item type or index out of range, return error */
	if (offset < 0)
		return -1;

	/* Make sure the caller hasn't asked for more data than we can handle */
	if (count > DM_MAX_DATA_SIZE)
		return -1;

	/* Read the prefix and data */
	len = -1;

	if (lseek(g_task_fd, offset, SEEK_SET) == offset)
		len = read(g_task_fd, buffer, count + DM_SECTOR_HDR_SIZE);

	/* Check for read error */
	if (len < 0)
		return -1;

	/* A zero length entry is a empty entry */
	if (len == 0)
		buffer[0] = 0;

	/* See if we got data */
	if (buffer[0] > 0) {
		/* We got more than requested!!! */
		if (buffer[0] > count)
			return -1;

		/* Looks good, copy it to the caller's buffer */
		memcpy(buf, buffer + DM_SECTOR_HDR_SIZE, buffer[0]);
	}

	/* Return the number of bytes of caller data read */
	return buffer[0];
}

static int
_clear(dm_item_t item)
{
	int i, result = 0;

	/* Get the offset of 1st item of this type */
	int offset = calculate_offset(item, 0);

	/* Check for item type out of range */
	if (offset < 0)
		return -1;

	/* Clear all items of this type */
	for (i = 0; (unsigned)i < g_per_item_max_index[item]; i++) {
		char buf[1];

		if (lseek(g_task_fd, offset, SEEK_SET) != offset) {
			result = -1;
			break;
		}

		/* Avoid SD flash wear by only doing writes where necessary */
		if (read(g_task_fd, buf, 1) < 1)
			break;

		/* If item has length greater than 0 it needs to be overwritten */
		if (buf[0]) {
			if (lseek(g_task_fd, offset, SEEK_SET) != offset) {
				result = -1;
				break;
			}

			buf[0] = 0;

			if (write(g_task_fd, buf, 1) != 1) {
				result = -1;
				break;
			}
		}

		offset += k_sector_size;
	}

	/* Make sure data is actually written to physical media */
	fsync(g_task_fd);
	return result;
}

/* Tell the data manager about the type of the last reset */
static int
_restart(dm_reset_reason reason)
{
	unsigned char buffer[2];
	int offset, result = 0;

	/* We need to scan the entire file and invalidate and data that should not persist after the last reset */

	/* Loop through all of the data segments and delete those that are not persistent */
	offset = 0;

	while (1) {
		size_t len;

		/* Get data segment at current offset */
		if (lseek(g_task_fd, offset, SEEK_SET) != offset) {
			result = -1;
			break;
		}

		len = read(g_task_fd, buffer, sizeof(buffer));

		if (len == 0)
			break;

		/* check if segment contains data */
		if (buffer[0]) {
			int clear_entry = 0;

			/* Whether data gets deleted depends on reset type and data segment's persistence setting */
			if (reason == DM_INIT_REASON_POWER_ON) {
				if (buffer[1] != DM_PERSIST_POWER_ON_RESET) {
					clear_entry = 1;
				}

			} else {
				if ((buffer[1] != DM_PERSIST_POWER_ON_RESET) && (buffer[1] != DM_PERSIST_IN_FLIGHT_RESET)) {
					clear_entry = 1;
				}
			}

			/* Set segment to unused if data does not persist */
			if (clear_entry) {
				if (lseek(g_task_fd, offset, SEEK_SET) != offset) {
					result = -1;
					break;
				}

				buffer[0] = 0;

				len = write(g_task_fd, buffer, 1);

				if (len != 1) {
					result = -1;
					break;
				}
			}
		}

		offset += k_sector_size;
	}

	fsync(g_task_fd);

	/* tell the caller how it went */
	return result;
}

/* 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)
{
	work_q_item_t *work;

	/* Make sure data manager has been started and is not shutting down */
	if ((g_fd < 0) || g_task_should_exit)
		return -1;

	/* get a work item and queue up a write request */
	if ((work = create_work_item()) == NULL)
		return -1;

	work->func = dm_write_func;
	work->write_params.item = item;
	work->write_params.index = index;
	work->write_params.persistence = persistence;
	work->write_params.buf = buf;
	work->write_params.count = count;

	/* Enqueue the item on the work queue and wait for the worker thread to complete processing it */
	return (ssize_t)enqueue_work_item_and_wait_for_result(work);
}

/* Retrieve from the data manager file */
__EXPORT ssize_t
dm_read(dm_item_t item, unsigned char index, void *buf, size_t count)
{
	work_q_item_t *work;

	/* Make sure data manager has been started and is not shutting down */
	if ((g_fd < 0) || g_task_should_exit)
		return -1;

	/* get a work item and queue up a read request */
	if ((work = create_work_item()) == NULL)
		return -1;

	work->func = dm_read_func;
	work->read_params.item = item;
	work->read_params.index = index;
	work->read_params.buf = buf;
	work->read_params.count = count;

	/* Enqueue the item on the work queue and wait for the worker thread to complete processing it */
	return (ssize_t)enqueue_work_item_and_wait_for_result(work);
}

__EXPORT int
dm_clear(dm_item_t item)
{
	work_q_item_t *work;

	/* Make sure data manager has been started and is not shutting down */
	if ((g_fd < 0) || g_task_should_exit)
		return -1;

	/* get a work item and queue up a clear request */
	if ((work = create_work_item()) == NULL)
		return -1;

	work->func = dm_clear_func;
	work->clear_params.item = item;

	/* Enqueue the item on the work queue and wait for the worker thread to complete processing it */
	return enqueue_work_item_and_wait_for_result(work);
}

/* Tell the data manager about the type of the last reset */
__EXPORT int
dm_restart(dm_reset_reason reason)
{
	work_q_item_t *work;

	/* Make sure data manager has been started and is not shutting down */
	if ((g_fd < 0) || g_task_should_exit)
		return -1;

	/* get a work item and queue up a restart request */
	if ((work = create_work_item()) == NULL)
		return -1;

	work->func = dm_restart_func;
	work->restart_params.reason = reason;

	/* Enqueue the item on the work queue and wait for the worker thread to complete processing it */
	return enqueue_work_item_and_wait_for_result(work);
}

static int
task_main(int argc, char *argv[])
{
	work_q_item_t *work;

	/* inform about start */
	warnx("Initializing..");

	/* Initialize global variables */
	g_key_offsets[0] = 0;

	for (unsigned i = 0; i < (DM_KEY_NUM_KEYS - 1); i++)
		g_key_offsets[i + 1] = g_key_offsets[i] + (g_per_item_max_index[i] * k_sector_size);

	unsigned max_offset = g_key_offsets[DM_KEY_NUM_KEYS - 1] + (g_per_item_max_index[DM_KEY_NUM_KEYS - 1] * k_sector_size);

	for (unsigned i = 0; i < dm_number_of_funcs; i++)
		g_func_counts[i] = 0;

	g_task_should_exit = false;

	init_q(&g_work_q);
	init_q(&g_free_q);

	sem_init(&g_work_queued_sema, 1, 0);

	/* See if the data manage file exists and is a multiple of the sector size */
	g_task_fd = open(k_data_manager_device_path, O_RDONLY | O_BINARY);
	if (g_task_fd >= 0) {
		/* File exists, check its size */
		int file_size = lseek(g_task_fd, 0, SEEK_END);
		if ((file_size % k_sector_size) != 0) {
			warnx("Incompatible data manager file %s, resetting it", k_data_manager_device_path);
			close(g_task_fd);
			unlink(k_data_manager_device_path);
		}
		else
			close(g_task_fd);
	}

	/* Open or create the data manager file */
	g_task_fd = open(k_data_manager_device_path, O_RDWR | O_CREAT | O_BINARY);

	if (g_task_fd < 0) {
		warnx("Could not open data manager file %s", k_data_manager_device_path);
		sem_post(&g_init_sema); /* Don't want to hang startup */
		return -1;
	}

	if ((unsigned)lseek(g_task_fd, max_offset, SEEK_SET) != max_offset) {
		close(g_task_fd);
		warnx("Could not seek data manager file %s", k_data_manager_device_path);
		sem_post(&g_init_sema); /* Don't want to hang startup */
		return -1;
	}

	fsync(g_task_fd);

	/* We use two file descriptors, one for the caller context and one for the worker thread */
	/* They are actually the same but we need to some way to reject caller request while the */
	/* worker thread is shutting down but still processing requests */
	g_fd = g_task_fd;

	warnx("Initialized, data manager file '%s' size is %d bytes", k_data_manager_device_path, max_offset);

	/* Tell startup that the worker thread has completed its initialization */
	sem_post(&g_init_sema);

	/* Start the endless loop, waiting for then processing work requests */
	while (true) {

		/* do we need to exit ??? */
		if ((g_task_should_exit) && (g_fd >= 0)) {
			/* Close the file handle to stop further queuing */
			g_fd = -1;
		}

		if (!g_task_should_exit) {
			/* wait for work */
			sem_wait(&g_work_queued_sema);
		}

		/* Empty the work queue */
		while ((work = dequeue_work_item())) {

			/* handle each work item with the appropriate handler */
			switch (work->func) {
			case dm_write_func:
				g_func_counts[dm_write_func]++;
				work->result =
					_write(work->write_params.item, work->write_params.index, work->write_params.persistence, work->write_params.buf, work->write_params.count);
				break;

			case dm_read_func:
				g_func_counts[dm_read_func]++;
				work->result =
					_read(work->read_params.item, work->read_params.index, work->read_params.buf, work->read_params.count);
				break;

			case dm_clear_func:
				g_func_counts[dm_clear_func]++;
				work->result = _clear(work->clear_params.item);
				break;

			case dm_restart_func:
				g_func_counts[dm_restart_func]++;
				work->result = _restart(work->restart_params.reason);
				break;

			default: /* should never happen */
				work->result = -1;
				break;
			}

			/* Inform the caller that work is done */
			sem_post(&work->wait_sem);
		}

		/* time to go???? */
		if ((g_task_should_exit) && (g_fd < 0))
			break;
	}

	close(g_task_fd);
	g_task_fd = -1;

	/* The work queue is now empty, empty the free queue */
	for (;;) {
		if ((work = (work_q_item_t *)sq_remfirst(&(g_free_q.q))) == NULL)
			break;

		free(work);
	}

	destroy_q(&g_work_q);
	destroy_q(&g_free_q);
	sem_destroy(&g_work_queued_sema);

	return 0;
}

static int
start(void)
{
	int task;

	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) {
		warn("task start failed");
		return -1;
	}

	/* wait for the thread to actuall initialize */
	sem_wait(&g_init_sema);
	sem_destroy(&g_init_sema);

	return 0;
}

static void
status(void)
{
	/* display usage statistics */
	warnx("Writes   %d", g_func_counts[dm_write_func]);
	warnx("Reads    %d", g_func_counts[dm_read_func]);
	warnx("Clears   %d", g_func_counts[dm_clear_func]);
	warnx("Restarts %d", g_func_counts[dm_restart_func]);
	warnx("Max Q lengths work %d, free %d", g_work_q.max_size, g_free_q.max_size);
}

static void
stop(void)
{
	/* Tell the worker task to shut down */
	g_task_should_exit = true;
	sem_post(&g_work_queued_sema);
}

static void
usage(void)
{
	errx(1, "usage: dataman {start|stop|status|poweronrestart|inflightrestart}");
}

int
dataman_main(int argc, char *argv[])
{
	if (argc < 2)
		usage();

	if (!strcmp(argv[1], "start")) {

		if (g_fd >= 0)
			errx(1, "already running");

		start();

		if (g_fd < 0)
			errx(1, "start failed");

		exit(0);
	}

	/* Worker thread should be running for all other commands */
	if (g_fd < 0)
		errx(1, "not running");

	if (!strcmp(argv[1], "stop"))
		stop();
	else if (!strcmp(argv[1], "status"))
		status();
	else if (!strcmp(argv[1], "poweronrestart"))
		dm_restart(DM_INIT_REASON_POWER_ON);
	else if (!strcmp(argv[1], "inflightrestart"))
		dm_restart(DM_INIT_REASON_IN_FLIGHT);
	else
		usage();

	exit(1);
}