Add support for setting and exporting parameters.

apps/px4/tests/tests_param.c

@@ -52,19 +52,29 @@ test_param(int argc, char *argv[])
 
 	p = param_find("test");
 	if (p == PARAM_INVALID)
-		errx(1, "test parameter not found\n");
+		errx(1, "test parameter not found");
 
 	param_type_t t = param_type(p);
 	if (t != PARAM_TYPE_INT32)
-		errx(1, "test parameter type mismatch (got %u)\n", (unsigned)t);
+		errx(1, "test parameter type mismatch (got %u)", (unsigned)t);
 
 	int32_t	val;
 	if (param_get(p, &val) != 0)
-		errx(1, "failed to read test parameter\n");
+		errx(1, "failed to read test parameter");
 	if (val != 0x12345678)
-		errx(1, "parameter value mismatch\n");
+		errx(1, "parameter value mismatch");
 
-	warnx("parameter test PASS\n");
+	val = 0xa5a5a5a5;
+	if (param_set(p, &val) != 0)
+		errx(1, "failed to write test parameter");
+	if (param_get(p, &val) != 0)
+		errx(1, "failed to re-read test parameter");
+	if (val != 0xa5a5a5a5)
+		errx(1, "parameter value mismatch after write");
+
+	param_export(NULL);
+
+	warnx("parameter test PASS");
 
 	return 0;
 }

apps/systemlib/Makefile

@@ -38,7 +38,9 @@
 CSRCS		 = err.c \
 		   hx_stream.c \
 		   perf_counter.c \
-		   param/param.c
+		   param/param.c \
+		   bson/bson.c \
+		   bson/bcon.c
 
 #
 # XXX this really should be a CONFIG_* test

apps/systemlib/param/param.c

@@ -6,10 +6,13 @@
 
 #include <string.h>
 #include <stdbool.h>
-
+#include <fcntl.h>
 #include <unistd.h>
+#include <err.h>
 
-#include "param.h"
+#include "systemlib/param/param.h"
+#include "systemlib/uthash/utarray.h"
+#include "systemlib/bson/bson.h"
 
 /**
  * Array of static parameter info.
@@ -19,12 +22,78 @@ static const struct param_info_s	*param_info_base = (struct param_info_s *)&__pa
 static const struct param_info_s	*param_info_limit = (struct param_info_s *)&__param_end;
 #define	param_info_count		((unsigned)(param_info_limit - param_info_base))
 
+/**
+ * Storage for modified parameters.
+ */
+struct param_wbuf_s
+{
+	param_t			param;
+	union param_value_u	val;
+	bool			unsaved;
+};
+
+UT_icd		param_icd = {sizeof(struct param_wbuf_s), NULL, NULL, NULL};
+UT_array	*param_values;
+
+static void
+param_lock(void)
+{
+	/* XXX */
+}
+
+static void
+param_unlock(void)
+{
+	/* XXX */
+}
+
+static void
+param_assert_locked(void)
+{
+	/* XXX */
+}
+
 static bool
 handle_in_range(param_t handle) 
 {
 	return (handle < param_info_count);
 }
 
+static int
+param_compare_values(const void *a, const void *b)
+{
+	struct param_wbuf_s *pa = (struct param_wbuf_s *)a;
+	struct param_wbuf_s *pb = (struct param_wbuf_s *)b;
+
+	if (pa->param < pb->param)
+		return -1;
+	if (pa->param > pb->param)
+		return 1;
+	return 0;
+}
+
+static struct param_wbuf_s *
+param_find_changed(param_t param)
+{
+	struct param_wbuf_s	*s = NULL;
+
+	param_assert_locked();
+
+	if (param_values != NULL) {
+#if 0	/* utarray_find requires bsearch, not available */
+		struct param_wbuf_s key;
+		key.param = param;
+		s = utarray_find(param_values, &key, param_compare_values);
+#else
+		while ((s = (struct param_wbuf_s *)utarray_next(param_values, s)) != NULL) {
+			if (s->param == param)
+				break;
+		}
+#endif
+	}
+	return s;
+}
+
 param_t
 param_find(const char *name)
 {
@@ -39,8 +108,17 @@ param_find(const char *name)
 	return PARAM_INVALID;
 }
 
-enum
+const char *
-param_type_e param_type(param_t param)
+param_name(param_t param)
+{
+	if (handle_in_range(param))
+		return param_info_base[param].name;
+
+	return NULL;	
+}
+
+enum param_type_e 
+param_type(param_t param)
 {
 	if (handle_in_range(param))
 		return param_info_base[param].type;
@@ -51,7 +129,6 @@ param_type_e param_type(param_t param)
 size_t
 param_size(param_t param)
 {
-
 	if (handle_in_range(param)) {
 		switch (param_info_base[param].type) {
 		case PARAM_TYPE_INT32:
@@ -72,39 +149,193 @@ param_size(param_t param)
 int
 param_get(param_t param, void *val)
 {
+	int result = -1;
+
+	param_lock();
+
 	if (handle_in_range(param)) {
 
+		struct param_wbuf_s *s = param_find_changed(param);
+		const union param_value_u *v;
+
+		/* work out whether we're fetching the default or a written value */
+		if (s != NULL) {
+			v = &s->val;
+		} else {
+			v = &param_info_base[param].val;
+		}
+
 		/* XXX look for updated value stored elsewhere */
 
 		switch (param_info_base[param].type) {
 		case PARAM_TYPE_INT32:
-			*(int32_t *)val = param_info_base[param].i;
+			*(int32_t *)val = v->i;
-			return 0;
+			result = 0;
+			break;
 
 		case PARAM_TYPE_FLOAT:
-			*(float *)val = param_info_base[param].f;
+			*(float *)val = v->f;
-			return 0;
+			result = 0;
+			break;
 
 		case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX:
-			memcpy(val, param_info_base[param].p, param_size(param));
+			*(void **)val = v->p;
-			return 0;
+			result = 0;
+			break;
 
 		default:
-			return -1;
+			break;
 		}
+
 	}
-	return -1;
+
+	param_unlock();
+
+	return result;
 }
 
 int
 param_set(param_t param, void *val)
 {
+	int result = -1;
+
+	param_lock();
+
+	if (param_values == NULL)
+		utarray_new(param_values, &param_icd);
+	if (param_values == NULL)
+		goto out;
+
 	if (handle_in_range(param)) {
 
-		/* XXX maintain list of changed values */
+		struct param_wbuf_s *s = param_find_changed(param);
 
+		if (s == NULL) {
+
+			/* construct a new parameter */
+			struct param_wbuf_s buf = { .param = param };
+
+			/* add it to the array and sort */
+			utarray_push_back(param_values, &buf);
+			utarray_sort(param_values, param_compare_values);
+
+			/* find it after sorting */
+			s = param_find_changed(param);
 		}
-	return -1;
 
+		/* update the changed value */
+		switch (param_info_base[param].type) {
+		case PARAM_TYPE_INT32:
+			s->val.i = *(int32_t *)val;
+			break;
+
+		case PARAM_TYPE_FLOAT:
+			s->val.f = *(float *)val;
+			break;
+
+		case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX:
+			s->val.p = *(void **)val;
+			break;
+
+		default:
+			goto out;
+		}
+		s->unsaved = true;
+
+		result = 0;
+	}
+out:
+	param_unlock();
+	return result;
 }
 
+int
+param_export(const char *filename, bool only_unsaved)
+{
+	struct param_wbuf_s *s;
+	bson	b[1];
+	int	result = -1;
+
+	param_lock();
+
+	bson_init(b);
+
+	while ((s = (struct param_wbuf_s *)utarray_next(param_values, s)) != NULL) {
+		const struct param_info_s *is = &param_info_base[s->param];
+
+		/* 
+		 * if we are only saving values changed since last save, and this
+		 * one hasn't, then skip it
+		 */
+		if (only_unsaved & !s->unsaved)
+			continue;
+		s->unsaved = false;
+
+		switch (is->type) {
+		case PARAM_TYPE_INT32:
+			if (bson_append_int(b, is->name, s->val.i) != BSON_OK)
+				goto out;
+			break;
+
+		case PARAM_TYPE_FLOAT:
+			if (bson_append_double(b, is->name, s->val.f) != BSON_OK)
+				goto out;
+			break;
+
+		case PARAM_TYPE_STRUCT ... PARAM_TYPE_STRUCT_MAX:
+			if (bson_append_binary(b, 
+					       is->name,
+					       is->type,
+					       is->val.p,
+					       is->type - PARAM_TYPE_STRUCT) != BSON_OK)
+				goto out;
+			break;
+		default:
+			goto out;
+		}
+	}
+	result = 0;
+
+out:
+	param_unlock();
+
+	if (result == 0) {
+		bson_finish(b);
+		bson_print(b);
+
+		warnx("object is %d bytes", bson_buffer_size(b));
+
+		if (filename != NULL) {
+			int fd = open(filename, O_CREAT | O_WRONLY | O_TRUNC, 0666);
+			int siz = bson_buffer_size(b);
+			int len = write(fd, bson_data(b), siz);
+			close(fd);
+			if (len != siz)
+				result = -1;
+		}
+	}
+	bson_destroy(b);
+
+	return result;
+}
+
+int
+param_import(const char *filename)
+{
+	return -1;
+}
+
+void
+param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_changed)
+{
+	param_t	param;
+
+	for (param = 0; handle_in_range(param); param++) {
+
+		/* if requested, skip unchanged values */
+		if (only_changed && (param_find_changed(param) == NULL))
+			continue;
+
+		func(arg, param);
+	}
+}

apps/systemlib/param/param.h

@@ -8,6 +8,7 @@
 #define _SYSTEMLIB_PARAM_PARAM_H
 
 #include <stdint.h>
+#include <stdbool.h>
 
 /**
  * Parameter types.
@@ -46,10 +47,18 @@ typedef uintptr_t	param_t;
  */
 __EXPORT param_t	param_find(const char *name);
 
+/**
+ * Obtain the name of a parameter.
+ *
+ * @param param		A handle returned by param_find or passed by param_foreach.
+ * @return		The name assigned to the parameter, or NULL if the handle is invalid.
+ */
+__EXPORT const char	*param_name(param_t param);
+
 /**
  * Obtain the type of a parameter.
  *
- * @param param		A handle returned by param_find.
+ * @param param		A handle returned by param_find or passed by param_foreach.
  * @return		The type assigned to the parameter.
  */
 __EXPORT param_type_t	param_type(param_t param);
@@ -57,7 +66,7 @@ __EXPORT param_type_t	param_type(param_t param);
 /**
  * Determine the size of a parameter.
  *
- * @param param		A handle returned by param_find.
+ * @param param		A handle returned by param_find or passed by param_foreach.
  * @return		The size of the parameter's value.
  */
 __EXPORT size_t		param_size(param_t param);
@@ -65,7 +74,7 @@ __EXPORT size_t		param_size(param_t param);
 /**
  * Obtain the scalar value of a parameter.
  *
- * @param param		A handle returned by param_find.
+ * @param param		A handle returned by param_find or passed by param_foreach.
  * @param val		Where to return the value, assumed to point to suitable storage for the parameter type.
  *			For structures, a pointer to the structure is returned.
  * @return		Zero if the parameter's value could be returned as a scalar, nonzero otherwise.
@@ -75,13 +84,43 @@ __EXPORT int		param_get(param_t param, void *val);
 /**
  * Set the scalar value of a parameter.
  *
- * @param param		A handle returned by param_find.
+ * @param param		A handle returned by param_find or passed by param_foreach.
  * @param val		The value to set; assumed to point to a variable of the parameter type.
  *			For structures, the pointer is assumed to point to a copy of the structure.
  * @return		Zero if the parameter's value could be set from a scalar, nonzero otherwise.
  */
 __EXPORT int		param_set(param_t param, void *val);
 
+/**
+ * Export changed parameters to a file.
+ *
+ * @param filename	The name of the file to export to.  If it exists, it will be overwritten.
+ * @param only_unsaved	Only export changed parameters that have not yet been exported.
+ * @return		Zero on success, nonzero on failure.
+ */
+__EXPORT int		param_export(const char *filename, bool only_unsaved);
+
+/**
+ * Import parameters from a file, discarding any unrecognised parameters.
+ *
+ * @param filename	The name of the file to import from.
+ * @return		Zero on success, nonzero if an error occurred during import.
+ *			Note that in the failure case, parameters may be inconsistent.
+ */
+__EXPORT int		param_import(const char *filename);
+
+/**
+ * Apply a function to each parameter.
+ *
+ * Note that the parameter set is not locked during the traversal.
+ *
+ * @param func		The function to invoke for each parameter.
+ * @param arg		Argument passed to the function.
+ * @param only_changed	If true, the function is only called for parameters whose values have
+ *			been changed from the default.
+ */
+__EXPORT void		param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_changed);
+
 /*
  * Macros creating static parameter definitions.
  *
@@ -97,7 +136,7 @@ __EXPORT int		param_set(param_t param, void *val);
 	struct param_info_s __param__##_name = {	\
 		.name = #_name,				\
 		.type = PARAM_TYPE_INT32,		\
-		.i = _default				\
+		.val.i = _default			\
 	}
 
 /** define a float parameter */
@@ -107,7 +146,7 @@ __EXPORT int		param_set(param_t param, void *val);
 	struct param_info_s __param__##_name = {	\
 		.name = #_name,				\
 		.type = PARAM_TYPE_FLOAT,		\
-		.f = _default				\
+		.val.f = _default			\
 	}
 
 /** define a parameter that points to a structure */
@@ -117,9 +156,19 @@ __EXPORT int		param_set(param_t param, void *val);
 	struct param_info_s __param__##_name = {	\
 		.name = #_name,				\
 		.type = PARAM_TYPE_STRUCT + sizeof(_default), \
-		.p = &_default;				\
+		.val.p = &_default;			\
 	}
 
+/**
+ * Parameter value union.
+ */
+union param_value_u
+{
+	void		*p;
+	int32_t		i;
+	float		f;	
+};
+
 /**
  * Static parameter definition structure.
  *
@@ -130,11 +179,7 @@ struct param_info_s
 {
 	const char	*name;
 	param_type_t	type;
-	union {
+	union param_value_u val;
-		void		*p;
-		int32_t		i;
-		float		f;
-	};
 };
 
 #endif