Work in progress on to/from memory BSON coding.

apps/px4/tests/test_bson.c

@@ -0,0 +1,187 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * 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 tests_bson.c
+ *
+ * Tests for the bson en/decoder
+ */
+
+#include <stdio.h>
+#include <systemlib/err.h>
+
+#include <systemlib/bson/tinybson.h>
+#include "tests.h"
+
+static const bool test_bool = true;
+static const int32_t test_int = 32;
+static const double test_double = 2.5;
+static const char *test_string = "this is a test";
+static const uint8_t test_data[256];
+static const char *test_filename = "/fs/microsd/bson.test";
+
+static int
+encode(bson_encoder_t encoder)
+{
+
+	if (bson_encoder_append_int(encoder, "thisisanillegalbsonpropertynamebecauseitistoolong", 0) == 0)
+		warnx("FAIL: encoder: too-long node name not rejected");
+
+	if (bson_encoder_append_int(encoder, "bool1", test_bool) != 0)
+		warnx("FAIL: encoder: append bool failed");
+	if (bson_encoder_append_int(encoder, "int1", test_int) != 0)
+		warnx("FAIL: encoder: append int failed");
+	if (bson_encoder_append_double(encoder, "double1", test_double) != 0)
+		warnx("FAIL: encoder: append double failed");
+	if (bson_encoder_append_string(encoder, "string1", test_string) != 0)
+		warnx("FAIL: encoder: append string failed");
+	if (bson_encoder_append_binary(encoder, "data1", test_data) != 0)
+		warnx("FAIL: encoder: append data failed");
+
+	bson_encoder_fini(encoder);
+
+	return 0;
+}
+
+static int
+decode_callback(bson_decoder_t decoder, void *private, bson_node_t node)
+{
+	if (!strcmp(node->name, "bool1")) {
+		if (node->b != test_bool)
+			warnx("FAIL: decoder: bool1 value %s, expected %s", 
+				(node->b ? "true" : "false"),
+				(test_bool ? "true" : "false"));
+		return 1;
+	}
+	if (!strcmp(node->name, "int1")) {
+		if (node->i != test_int)
+			warnx("FAIL: decoder: int1 value %d, expected %d", node->i, test_int);
+		return 1;
+	}
+	if (!strcmp(node->name, "double1")) {
+		if (node->d != test_double)
+			warnx("FAIL: decoder: double1 value %f, expected %f", node->d, test_double);
+		return 1;
+	}
+	if (!strcmp(node->name, "string1")) {
+		unsigned len = bson_decoder_data_pending(decoder);
+
+		if (len != (strlen(test_string) + 1)) {
+			warnx("FAIL: decoder: string1 length %d wrong, expected %d", len, strlen(test_string) + 1);
+			return 1;
+
+		char sbuf[len];
+
+		if (bson_decoder_copy_data(decoder, sbuf)) {
+			warnx("FAIL: decoder: string1 copy failed");
+			return 1;
+		}
+		if (bson_decoder_data_pending(decoder) != 0) {
+			warnx("FAIL: decoder: string1 copy did not exhaust all data");
+			return 1;
+		}
+		if (sbuf[len - 1] != '\0') {
+			warnx("FAIL: decoder: string1 not 0-terminated");
+			return 1;
+		}
+		if (strcmp(sbuf, test_string)) {
+			warnx("FAIL: decoder: string1 value '%s', expected '%s'", sbuf, test_string);
+			return 1;
+		}
+		return 1;
+	}
+	if (!strcmp(node->name, "data1")) {
+		unsigned len = bson_decoder_data_pending(decoder);
+
+		if (len != sizeof(test_data)) {
+			warnx("FAIL: decoder: data1 length %d, expected %d", len, sizeof(test_data));
+			return 1;
+		}
+		
+		uint8_t dbuf[len];
+
+		if (bson_decoder_copy_data(decoder, dbuf)) {
+			warnx("FAIL: decoder: data1 copy failed");
+			return 1;
+		}
+		if (bson_decoder_data_pending(decoder) != 0) {
+			warnx("FAIL: decoder: data1 copy did not exhaust all data");
+			return 1;
+		}
+		if (memcmp(test_data, dbuf, len)) {
+			warnx("FAIL: decoder: data1 compare fail");
+			return 1;
+		}
+		return 1;
+	}
+
+	warnx("FAIL: decoder: unexpected node name '%s'", node->name);
+	return 1;
+}
+
+static int
+decode(bson_decoder_t decoder)
+{
+	int result;
+
+	do {
+		result = bson_decoder_next(decoder);
+	} while (result > 0);
+}
+
+int
+test_bson(int argc, char *argv[])
+{
+	bson_encoder_t encoder;
+	bson_decoder_t decoder;
+	void *buf;
+	int len, fd;
+
+	/* encode data to a memory buffer */
+	if (bson_encoder_init_buf(&encoder, NULL, 0))
+		errx("FAIL: bson_encoder_init_buf");
+	encode(encoder);
+	len = bson_encoder_buf_size(encoder);
+	if (len <= 0)
+		errx("FAIL: bson_encoder_buf_len");
+	buf = bson_encoder_buf_data(encoder);
+
+	/* now test-decode it */
+	if (bson_decoder_init_buf(&decoder, buf, len))
+		errx("FAIL: bson_decoder_init_buf");
+	decode(decoder);
+	free(buf);
+
+	exit(0);
+}

apps/px4/tests/tests.h

@@ -96,6 +96,7 @@ extern int	test_time(int argc, char *argv[]);
 extern int	test_uart_console(int argc, char *argv[]);
 extern int	test_jig_voltages(int argc, char *argv[]);
 extern int	test_param(int argc, char *argv[]);
+extern int	test_bson(int argc, char *argv[]);
 extern int	test_file(int argc, char *argv[]);
 
 #endif /* __APPS_PX4_TESTS_H */

apps/px4/tests/tests_main.c

@@ -109,6 +109,7 @@ struct {
 	{"all",			test_all,	OPT_NOALLTEST | OPT_NOJIGTEST, 0},
 	{"jig",			test_jig,	OPT_NOJIGTEST | OPT_NOALLTEST, 0},
 	{"param",		test_param,	0, 0},
+	{"bson",		test_bson,	0, 0},
 	{"file",		test_file,	0, 0},
 	{"help",		test_help,	OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST, 0},
 	{NULL,			NULL, 		0, 0}

apps/px4/tests/tests_param.c

@@ -73,8 +73,6 @@ test_param(int argc, char *argv[])
 	if ((uint32_t)val != 0xa5a5a5a5)
 		errx(1, "parameter value mismatch after write");
 
-	param_export(-1, false);
-
 	warnx("parameter test PASS");
 
 	return 0;

apps/systemlib/bson/tinybson.c

@@ -43,40 +43,60 @@
 
 #include "tinybson.h"
 
-
 #if 0
 # define debug(fmt, args...)		do { warnx("BSON: " fmt, ##args); } while(0)
 #else
 # define debug(fmt, args...)		do { } while(0)
 #endif
 
-#define CODER_CHECK(_c)		do { if (_c->fd == -1) return -1; } while(0)
+#define CODER_CHECK(_c)		do { if (_c->dead) return -1; } while(0)
-#define CODER_KILL(_c, _reason)	do { debug("killed: %s", _reason); _c->fd = -1; return -1; } while(0)
+#define CODER_KILL(_c, _reason)	do { debug("killed: %s", _reason); _c->dead = true; return -1; } while(0)
+
+static int
+read_x(bson_decoder_t decoder, void *p, size_t s)
+{
+	CODER_CHECK(decoder);
+
+	if (decoder->fd > 0)
+		return (read(decoder->fd, p, s) == s) ? 0 : -1;
+
+	if (decoder->buf != NULL) {
+		unsigned newpos = decoder->bufpos + s;
+		if (newpos <= decoder->bufsize) {
+			memcpy(p, (decoder->buf + decoder->bufpos), s);
+			decoder->bufpos = newpos;
+			return 0;
+		}
+	}
+	return -1;
+}
 
 static int
 read_int8(bson_decoder_t decoder, int8_t *b)
 {
-	return (read(decoder->fd, b, sizeof(*b)) == sizeof(*b)) ? 0 : -1;
+	return read_x(decoder, b, sizeof(*b));
 }
 
 static int
 read_int32(bson_decoder_t decoder, int32_t *i)
 {
-	return (read(decoder->fd, i, sizeof(*i)) == sizeof(*i)) ? 0 : -1;
+	return read_x(decoder, i, sizeof(*i));
 }
 
 static int
 read_double(bson_decoder_t decoder, double *d)
 {
-	return (read(decoder->fd, d, sizeof(*d)) == sizeof(*d)) ? 0 : -1;
+	return read_x(decoder, d, sizeof(*d));
 }
 
 int
-bson_decoder_init(bson_decoder_t decoder, int fd, bson_decoder_callback callback, void *private)
+bson_decoder_init_file(bson_decoder_t decoder, int fd, bson_decoder_callback callback, void *private)
 {
 	int32_t	junk;
 
 	decoder->fd = fd;
+	decoder->buf = NULL;
+	decoder->dead = false;
 	decoder->callback = callback;
 	decoder->private = private;
 	decoder->nesting = 1;
@@ -91,6 +111,32 @@ bson_decoder_init(bson_decoder_t decoder, int fd, bson_decoder_callback callback
 	return 0;
 }
 
+int
+bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_decoder_callback callback, void *private)
+{
+	int32_t	len;
+
+	decoder->fd = -1;
+	decoder->buf = (uint8_t *)buf;
+	decoder->dead = false;
+	decoder->bufsize = bufsize;
+	decoder->bufpos = 0;
+	decoder->callback = callback;
+	decoder->private = private;
+	decoder->nesting = 1;
+	decoder->pending = 0;
+	decoder->node.type = BSON_UNDEFINED;
+
+	/* read and discard document size */
+	if (read_int32(decoder, &len))
+		CODER_KILL(decoder, "failed reading length");
+	if (len > bufsize)
+		CODER_KILL(decoder, "document length larger than buffer");
+
+	/* ready for decoding */
+	return 0;
+}
+
 int
 bson_decoder_next(bson_decoder_t decoder)
 {
@@ -154,6 +200,12 @@ bson_decoder_next(bson_decoder_t decoder)
 		debug("got name '%s'", decoder->node.name);
 
 		switch (decoder->node.type) {
+		case BSON_BOOL:
+			if (read_int8(decoder, &tbyte))
+				CODER_KILL(decoder, "read error on BSON_BOOL");
+			node->b = (tbyte != 0);
+			break;
+
 		case BSON_INT:
 			if (read_int32(decoder, &decoder->node.i))
 				CODER_KILL(decoder, "read error on BSON_INT");
@@ -199,14 +251,10 @@ bson_decoder_copy_data(bson_decoder_t decoder, void *buf)
 
 	CODER_CHECK(decoder);
 
-	/* if data already copied, return zero bytes */
+	/* copy data */
-	if (decoder->pending == 0)
+	result = read_x(decoder, buf, decoder->pending);
-		return 0;
 
-	/* copy bytes per the node size */
+	if (result != 0)
-	result = read(decoder->fd, buf, decoder->pending);
-
-	if (result != decoder->pending)
 		CODER_KILL(decoder, "read error on copy_data");
 
 	/* pending count is discharged */
@@ -220,25 +268,48 @@ bson_decoder_data_pending(bson_decoder_t decoder)
 	return decoder->pending;
 }
 
+static int
+write_x(bson_encoder_t encoder, void *p, size_t s)
+{
+	CODER_CHECK(encoder);
+
+	if (encoder->fd > -1)
+		return (write(encoder->fd, p, s) == s) ? 0 : -1;
+
+	/* do we need to extend the buffer? */
+	while ((encoder->bufpos + s) > encoder->bufsize) {
+		if (!encoder->realloc_ok)
+			CODER_KILL(encoder);
+
+		int8_t *newbuf = realloc(encoder->buf, encoder->bufsize + BSON_BUF_INCREMENT);
+		if (newbuf == NULL)
+			CODER_KILL(encoder);
+
+		encoder->bufsize += BSON_BUF_INCREMENT;
+	}
+
+	memcpy(encoder->buf + encoder->bufpos, p, s);
+	encoder->bufpos += s;
+
+	return 0;
+}
+
 static int
 write_int8(bson_encoder_t encoder, int8_t b)
 {
-	debug("write_int8 %d", b);
+	return write_x(encoder, &b, sizeof(b));
-	return (write(encoder->fd, &b, sizeof(b)) == sizeof(b)) ? 0 : -1;
 }
 
 static int
 write_int32(bson_encoder_t encoder, int32_t i)
 {
-	debug("write_int32 %d", i);
+	return write_x(encoder, &i, sizeof(i));
-	return (write(encoder->fd, &i, sizeof(i)) == sizeof(i)) ? 0 : -1;
 }
 
 static int
 write_double(bson_encoder_t encoder, double d)
 {
-	debug("write_double");
+	return write_x(encoder, &d, sizeof(d));
-	return (write(encoder->fd, &d, sizeof(d)) == sizeof(d)) ? 0 : -1;
 }
 
 static int
@@ -249,14 +320,15 @@ write_name(bson_encoder_t encoder, const char *name)
 	if (len > BSON_MAXNAME)
 		return -1;
 
-	debug("write name '%s' len %d", name, len);
+	return write_x(encoder, name, len + 1);
-	return (write(encoder->fd, name, len + 1) == (int)(len + 1)) ? 0 : -1;
 }
 
 int
-bson_encoder_init(bson_encoder_t encoder, int fd)
+bson_encoder_init_file(bson_encoder_t encoder, int fd)
 {
 	encoder->fd = fd;
+	encoder->buf = NULL;
+	encoder->dead = false;
 
 	if (write_int32(encoder, 0))
 		CODER_KILL(encoder, "write error on document length");
@@ -264,6 +336,27 @@ bson_encoder_init(bson_encoder_t encoder, int fd)
 	return 0;
 }
 
+int
+bson_encoder_init_buf(bson_encoder_t encoder, void *buf, unsigned bufsize)
+{
+	encoder->fd = -1;
+	encoder->buf = (uint8_t *)buf;
+	encoder->bufpos = 0;
+	encoder->dead = false;
+	if (encoder->buf == NULL) {
+		encoder->bufsize = 0;
+		encoder->realloc_ok = true;
+	} else {
+		encoder->bufsize = bufsize;
+		encoder->realloc_ok = false;
+	}
+
+	if (write_int32(encoder, 0))
+		CODER_KILL(encoder, "write error on document length");
+
+	return 0;		
+}
+
 int
 bson_encoder_fini(bson_encoder_t encoder)
 {
@@ -272,6 +365,46 @@ bson_encoder_fini(bson_encoder_t encoder)
 	if (write_int8(encoder, BSON_EOO))
 		CODER_KILL(encoder, "write error on document terminator");
 
+	/* hack to fix up length for in-buffer documents */
+	if (encoder->buf != NULL) {
+		int32_t len = bson_encoder_buf_size(encoder);
+		memcpy(encoder->buf, &len, sizeof(len));
+	}
+
+	return 0;
+}
+
+int
+bson_encoder_buf_size(bson_encoder_t encoder)
+{
+	CODER_CHECK(encoder);
+
+	if (encoder->fd > -1)
+		return -1;
+
+	return encoder->bufpos;
+}
+
+int
+bson_encoder_buf_data(bson_encoder_t encoder)
+{
+	/* note, no CODER_CHECK here as the caller has to clean up dead buffers */
+
+	if (encoder->fd > -1)
+		return NULL;
+
+	return encoder->buf;
+}
+
+int bson_encoder_append_bool(bson_encoder_t encoder, const char *name, bool value)
+{
+	CODER_CHECK(encoder);
+
+	if (write_int8(encoder, BSON_INT) ||
+	    write_name(encoder, name) ||
+	    write_int(encoder, value ? 1 : 0))
+		CODER_KILL(encoder, "write error on BSON_BOOL");
+
 	return 0;
 }
 
@@ -314,7 +447,7 @@ bson_encoder_append_string(bson_encoder_t encoder, const char *name, const char
 	if (write_int8(encoder, BSON_DOUBLE) ||
 	    write_name(encoder, name) ||
 	    write_int32(encoder, len) ||
-	    write(encoder->fd, name, len + 1) != (int)(len + 1))
+	    write_x(encoder, name, len + 1))
 		CODER_KILL(encoder, "write error on BSON_STRING");
 
 	return 0;
@@ -329,7 +462,7 @@ bson_encoder_append_binary(bson_encoder_t encoder, const char *name, bson_binary
 	    write_name(encoder, name) ||
 	    write_int32(encoder, size) ||
 	    write_int8(encoder, subtype) ||
-	    write(encoder->fd, data, size) != (int)(size))
+	    write_x(encoder, data, size))
 		CODER_KILL(encoder, "write error on BSON_BINDATA");
 
 	return 0;

apps/systemlib/bson/tinybson.h

@@ -74,6 +74,11 @@ typedef enum bson_binary_subtype {
  */
 #define BSON_MAXNAME		32
 
+/**
+ * Buffer growth increment when writing to a buffer.
+ */
+#define BSON_BUF_INCREMENT	128
+
 /**
  * Node structure passed to the callback.
  */
@@ -92,11 +97,21 @@ typedef struct bson_decoder_s *bson_decoder_t;
 
 /**
  * Node callback.
+ *
+ * The node callback function's return value is returned by bson_decoder_next.
  */
 typedef int	(* bson_decoder_callback)(bson_decoder_t decoder, void *private, bson_node_t node);
 
 struct bson_decoder_s {
+	/* file reader state */
 	int			fd;
+
+	/* buffer reader state */
+	uint8_t			*buf;
+	size_t			bufsize;
+	unsigned		bufpos;
+
+	bool			dead;
 	bson_decoder_callback	callback;
 	void			*private;
 	unsigned		nesting;
@@ -105,7 +120,7 @@ struct bson_decoder_s {
 };
 
 /**
- * Initialise the decoder.
+ * Initialise the decoder to read from a file.
  *
  * @param decoder		Decoder state structure to be initialised.
  * @param fd			File to read BSON data from.
@@ -113,7 +128,19 @@ struct bson_decoder_s {
  * @param private		Callback private data, stored in node.
  * @return			Zero on success.
  */
-__EXPORT int bson_decoder_init(bson_decoder_t decoder, int fd, bson_decoder_callback callback, void *private);
+__EXPORT int bson_decoder_init_file(bson_decoder_t decoder, int fd, bson_decoder_callback callback, void *private);
+
+/**
+ * Initialise the decoder to read from a buffer in memory.
+ *
+ * @param decoder		Decoder state structure to be initialised.
+ * @param buf			Buffer to read from.
+ * @param bufsize		Size of the buffer (BSON object may be smaller).
+ * @param callback		Callback to be invoked by bson_decoder_next
+ * @param private		Callback private data, stored in node.
+ * @return			Zero on success.
+ */
+__EXPORT int bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_decoder_callback callback, void *private);
 
 /**
  * Process the next node from the stream and invoke the callback.
@@ -142,20 +169,65 @@ __EXPORT size_t bson_decoder_data_pending(bson_decoder_t decoder);
  * Encoder state structure.
  */
 typedef struct bson_encoder_s {
+	/* file writer state */
 	int		fd;
 
+	/* buffer writer state */
+	uint8_t		*buf;
+	unsigned	bufsize;
+	unsigned	bufpos;
+
+	bool		realloc_ok;
+	bool		dead;
+
 } *bson_encoder_t;
 
 /**
- * Initialze the encoder.
+ * Initialze the encoder for writing to a file.
+ *
+ * @param encoder		Encoder state structure to be initialised.
+ * @param fd			File to write to.
+ * @return			Zero on success.
  */
-__EXPORT int bson_encoder_init(bson_encoder_t encoder, int fd);
+__EXPORT int bson_encoder_init_file(bson_encoder_t encoder, int fd);
+
+/**
+ * Initialze the encoder for writing to a buffer.
+ *
+ * @param encoder		Encoder state structure to be initialised.
+ * @param buf			Buffer pointer to use, or NULL if the buffer
+ *				should be allocated by the encoder.
+ * @param bufsize		Maximum buffer size, or zero for no limit. If 
+ *				the buffer is supplied, the size of the supplied buffer.
+ * @return			Zero on success.
+ */
+__EXPORT int bson_encoder_init_buf(bson_encoder_t encoder, void *buf, unsigned bufsize);
 
 /**
  * Finalise the encoded stream.
+ *
+ * @param encoder		The encoder to finalise.
  */
 __EXPORT int bson_encoder_fini(bson_encoder_t encoder);
 
+/**
+ * Fetch the size of the encoded object; only valid for buffer operations.
+ */
+__EXPORT int bson_encoder_buf_size(bson_encoder_t encoder);
+
+/**
+ * Get a pointer to the encoded object buffer.
+ *
+ * Note that if the buffer was allocated by the encoder, it is the caller's responsibility
+ * to free this buffer.
+ */
+__EXPORT void *bson_encoder_buf_data(bson_encoder_t encoder);
+
+/**
+ * Append a boolean to the encoded stream.
+ */
+__EXPORT int bson_encoder_append_bool(bson_encoder_t encoder, const char *name, bool value);
+
 /**
  * Append an integer to the encoded stream.
  */