Merge branch 'master' of https://github.com/PX4/Firmware into fw_control

2 files changed, 320 insertions, 44 deletions

diff --git a/apps/systemlib/bson/tinybson.c b/apps/systemlib/bson/tinybson.c
index 75578d2ec..321466f87 100644
--- a/apps/systemlib/bson/tinybson.c
+++ b/apps/systemlib/bson/tinybson.c
@@ -39,44 +39,74 @@
 
 #include <unistd.h>
 #include <string.h>
+#include <stdlib.h>
 #include <err.h>
 
 #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_KILL(_c, _reason)	do { debug("killed: %s", _reason); _c->fd = -1; return -1; } while(0)
+#define CODER_CHECK(_c)		do { if (_c->dead) { debug("coder dead"); 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 > -1)
+		return (read(decoder->fd, p, s) == (int)s) ? 0 : -1;
+
+	if (decoder->buf != NULL) {
+		/* staged operations to avoid integer overflow for corrupt data */
+		if (s >= decoder->bufsize)
+			CODER_KILL(decoder, "buffer too small for read");
+		if ((decoder->bufsize - s) < decoder->bufpos)
+			CODER_KILL(decoder, "not enough data for read");
+		memcpy(p, (decoder->buf + decoder->bufpos), s);
+		decoder->bufpos += s;
+		return 0;
+	}
+	debug("no source");
+	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_int64(bson_decoder_t decoder, int64_t *i)
+{
+	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;
@@ -85,7 +115,42 @@ bson_decoder_init(bson_decoder_t decoder, int fd, bson_decoder_callback callback
 
 	/* read and discard document size */
 	if (read_int32(decoder, &junk))
+		CODER_KILL(decoder, "failed discarding length");
+
+	/* ready for decoding */
+	return 0;
+}
+
+int
+bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_decoder_callback callback, void *private)
+{
+	int32_t	len;
+
+	/* argument sanity */
+	if ((buf == NULL) || (callback == NULL))
+		return -1;
+
+	decoder->fd = -1;
+	decoder->buf = (uint8_t *)buf;
+	decoder->dead = false;
+	if (bufsize == 0) {
+		decoder->bufsize = *(uint32_t *)buf;
+		debug("auto-detected %u byte object", decoder->bufsize);
+	} else {
+		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 > 0) && (len > (int)decoder->bufsize))
+		CODER_KILL(decoder, "document length larger than buffer");
 
 	/* ready for decoding */
 	return 0;
@@ -95,6 +160,7 @@ int
 bson_decoder_next(bson_decoder_t decoder)
 {
 	int8_t	tbyte;
+	int32_t tint;
 	unsigned nlen;
 
 	CODER_CHECK(decoder);
@@ -133,7 +199,9 @@ bson_decoder_next(bson_decoder_t decoder)
 	debug("got type byte 0x%02x", decoder->node.type);
 
 	/* EOO is special; it has no name/data following */
-	if (decoder->node.type != BSON_EOO) {
+	if (decoder->node.type == BSON_EOO) {
+		decoder->node.name[0] = '\0';
+	} else {
 
 		/* get the node name */
 		nlen = 0;
@@ -154,8 +222,20 @@ bson_decoder_next(bson_decoder_t decoder)
 		debug("got name '%s'", decoder->node.name);
 
 		switch (decoder->node.type) {
-		case BSON_INT:
-			if (read_int32(decoder, &decoder->node.i))
+		case BSON_BOOL:
+			if (read_int8(decoder, &tbyte))
+				CODER_KILL(decoder, "read error on BSON_BOOL");
+			decoder->node.b = (tbyte != 0);
+			break;
+
+		case BSON_INT32:
+			if (read_int32(decoder, &tint))
+				CODER_KILL(decoder, "read error on BSON_INT");
+			decoder->node.i = tint;
+			break;
+
+		case BSON_INT64:
+			if (read_int64(decoder, &decoder->node.i))
 				CODER_KILL(decoder, "read error on BSON_INT");
 
 			break;
@@ -169,7 +249,6 @@ bson_decoder_next(bson_decoder_t decoder)
 		case BSON_STRING:
 			if (read_int32(decoder, &decoder->pending))
 				CODER_KILL(decoder, "read error on BSON_STRING length");
-
 			break;
 
 		case BSON_BINDATA:
@@ -199,14 +278,10 @@ bson_decoder_copy_data(bson_decoder_t decoder, void *buf)
 
 	CODER_CHECK(decoder);
 
-	/* if data already copied, return zero bytes */
-	if (decoder->pending == 0)
-		return 0;
-
-	/* copy bytes per the node size */
-	result = read(decoder->fd, buf, decoder->pending);
+	/* copy data */
+	result = read_x(decoder, buf, decoder->pending);
 
-	if (result != decoder->pending)
+	if (result != 0)
 		CODER_KILL(decoder, "read error on copy_data");
 
 	/* pending count is discharged */
@@ -221,24 +296,56 @@ bson_decoder_data_pending(bson_decoder_t decoder)
 }
 
 static int
+write_x(bson_encoder_t encoder, const void *p, size_t s)
+{
+	CODER_CHECK(encoder);
+
+	if (encoder->fd > -1)
+		return (write(encoder->fd, p, s) == (int)s) ? 0 : -1;
+
+	/* do we need to extend the buffer? */
+	while ((encoder->bufpos + s) > encoder->bufsize) {
+		if (!encoder->realloc_ok)
+			CODER_KILL(encoder, "fixed-size buffer overflow");
+
+		uint8_t *newbuf = realloc(encoder->buf, encoder->bufsize + BSON_BUF_INCREMENT);
+		if (newbuf == NULL)
+			CODER_KILL(encoder, "could not grow buffer");
+
+		encoder->buf = newbuf;
+		encoder->bufsize += BSON_BUF_INCREMENT;
+		debug("allocated %d bytes", BSON_BUF_INCREMENT);
+	}
+
+	memcpy(encoder->buf + encoder->bufpos, p, s);
+	encoder->bufpos += s;
+	debug("appended %d bytes", s);
+
+	return 0;
+}
+
+static int
 write_int8(bson_encoder_t encoder, int8_t b)
 {
-	debug("write_int8 %d", b);
-	return (write(encoder->fd, &b, sizeof(b)) == sizeof(b)) ? 0 : -1;
+	return write_x(encoder, &b, sizeof(b));
 }
 
 static int
 write_int32(bson_encoder_t encoder, int32_t i)
 {
-	debug("write_int32 %d", i);
-	return (write(encoder->fd, &i, sizeof(i)) == sizeof(i)) ? 0 : -1;
+	return write_x(encoder, &i, sizeof(i));
+}
+
+static int
+write_int64(bson_encoder_t encoder, int64_t i)
+{
+	return write_x(encoder, &i, sizeof(i));
 }
 
 static int
 write_double(bson_encoder_t encoder, double d)
 {
-	debug("write_double");
-	return (write(encoder->fd, &d, sizeof(d)) == sizeof(d)) ? 0 : -1;
+	return write_x(encoder, &d, sizeof(d));
 }
 
 static int
@@ -247,16 +354,17 @@ write_name(bson_encoder_t encoder, const char *name)
 	size_t len = strlen(name);
 
 	if (len > BSON_MAXNAME)
-		return -1;
+		CODER_KILL(encoder, "node name too long");
 
-	debug("write name '%s' len %d", name, len);
-	return (write(encoder->fd, name, len + 1) == (int)(len + 1)) ? 0 : -1;
+	return write_x(encoder, name, len + 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");
@@ -265,6 +373,27 @@ bson_encoder_init(bson_encoder_t encoder, int fd)
 }
 
 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)
 {
 	CODER_CHECK(encoder);
@@ -272,17 +401,69 @@ 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_append_int(bson_encoder_t encoder, const char *name, int32_t value)
+bson_encoder_buf_size(bson_encoder_t encoder)
 {
 	CODER_CHECK(encoder);
 
-	if (write_int8(encoder, BSON_INT) ||
+	if (encoder->fd > -1)
+		return -1;
+
+	return encoder->bufpos;
+}
+
+void *
+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_BOOL) ||
 	    write_name(encoder, name) ||
-	    write_int32(encoder, value))
+	    write_int8(encoder, value ? 1 : 0))
+		CODER_KILL(encoder, "write error on BSON_BOOL");
+
+	return 0;
+}
+
+int
+bson_encoder_append_int(bson_encoder_t encoder, const char *name, int64_t value)
+{
+	bool result;
+
+	CODER_CHECK(encoder);
+
+	/* use the smallest encoding that will hold the value */
+	if (value == (int32_t)value) {
+		debug("encoding %lld as int32", value);
+		result = write_int8(encoder, BSON_INT32) ||
+			 write_name(encoder, name) ||
+			 write_int32(encoder, value);
+	} else {
+		debug("encoding %lld as int64", value);
+		result = write_int8(encoder, BSON_INT64) ||
+			 write_name(encoder, name) ||
+			 write_int64(encoder, value);
+	}
+	if (result)
 		CODER_KILL(encoder, "write error on BSON_INT");
 
 	return 0;
@@ -309,12 +490,12 @@ bson_encoder_append_string(bson_encoder_t encoder, const char *name, const char
 
 	CODER_CHECK(encoder);
 
-	len = strlen(string);
+	len = strlen(string) + 1;	/* include trailing nul */
 
-	if (write_int8(encoder, BSON_DOUBLE) ||
+	if (write_int8(encoder, BSON_STRING) ||
 	    write_name(encoder, name) ||
 	    write_int32(encoder, len) ||
-	    write(encoder->fd, name, len + 1) != (int)(len + 1))
+	    write_x(encoder, string, len))
 		CODER_KILL(encoder, "write error on BSON_STRING");
 
 	return 0;
@@ -329,7 +510,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;
diff --git a/apps/systemlib/bson/tinybson.h b/apps/systemlib/bson/tinybson.h
index b6229dc50..666f8191a 100644
--- a/apps/systemlib/bson/tinybson.h
+++ b/apps/systemlib/bson/tinybson.h
@@ -59,9 +59,8 @@ typedef enum {
 	BSON_BOOL = 8,
 	BSON_DATE = 9,
 	BSON_NULL = 10,
-	BSON_INT = 16,
-	BSON_TIMESTAMP = 17,
-	BSON_LONG = 18
+	BSON_INT32 = 16,
+	BSON_INT64 = 18
 } bson_type_t;
 
 typedef enum bson_binary_subtype {
@@ -75,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.
  */
 typedef struct bson_node_s {
@@ -82,7 +86,7 @@ typedef struct bson_node_s {
 	bson_type_t		type;
 	bson_binary_subtype_t	subtype;
 	union {
-		int32_t		i;
+		int64_t		i;
 		double		d;
 		bool		b;
 	};
@@ -92,11 +96,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 +119,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 +127,21 @@ 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).  May be
+ *				passed as zero if the buffer size should be extracted from the
+ *				BSON header only.
+ * @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,37 +170,104 @@ __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.
+ *
+ * @param encoder		Encoder state.
+ * @param name			Node name.
+ * @param value			Value to be encoded.
+ */
+__EXPORT int bson_encoder_append_bool(bson_encoder_t encoder, const char *name, bool value);
+
+/**
  * Append an integer to the encoded stream.
+ *
+ * @param encoder		Encoder state.
+ * @param name			Node name.
+ * @param value			Value to be encoded. 
  */
-__EXPORT int bson_encoder_append_int(bson_encoder_t encoder, const char *name, int32_t value);
+__EXPORT int bson_encoder_append_int(bson_encoder_t encoder, const char *name, int64_t value);
 
 /**
  * Append a double to the encoded stream
+ *
+ * @param encoder		Encoder state.
+ * @param name			Node name.
+ * @param value			Value to be encoded.
  */
 __EXPORT int bson_encoder_append_double(bson_encoder_t encoder, const char *name, double value);
 
 /**
  * Append a string to the encoded stream.
+ *
+ * @param encoder		Encoder state.
+ * @param name			Node name.
+ * @param string		Nul-terminated C string.
  */
 __EXPORT int bson_encoder_append_string(bson_encoder_t encoder, const char *name, const char *string);
 
 /**
  * Append a binary blob to the encoded stream.
+ *
+ * @param encoder		Encoder state.
+ * @param name			Node name.
+ * @param subtype		Binary data subtype.
+ * @param size			Data size.
+ * @param data			Buffer containing data to be encoded.
  */
 __EXPORT int bson_encoder_append_binary(bson_encoder_t encoder, const char *name, bson_binary_subtype_t subtype, size_t size, const void *data);