1 files changed, 517 insertions, 0 deletions
diff --git a/src/modules/systemlib/bson/tinybson.c b/src/modules/systemlib/bson/tinybson.c
new file mode 100644
index 000000000..8aca6a25d
--- /dev/null
+++ b/src/modules/systemlib/bson/tinybson.c
@@ -0,0 +1,517 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * 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 tinybson.c
+ *
+ * A simple subset SAX-style BSON parser and generator.
+ */
+
+#include <unistd.h>
+#include <string.h>
+#include <stdlib.h>
+#include <systemlib/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->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_x(decoder, b, sizeof(*b));
+}
+
+static int
+read_int32(bson_decoder_t decoder, int32_t *i)
+{
+	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_x(decoder, d, sizeof(*d));
+}
+
+int
+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;
+	decoder->pending = 0;
+	decoder->node.type = BSON_UNDEFINED;
+
+	/* 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;
+}
+
+int
+bson_decoder_next(bson_decoder_t decoder)
+{
+	int8_t	tbyte;
+	int32_t tint;
+	unsigned nlen;
+
+	CODER_CHECK(decoder);
+
+	/* if the previous node was EOO, pop a nesting level */
+	if (decoder->node.type == BSON_EOO) {
+		if (decoder->nesting > 0)
+			decoder->nesting--;
+
+		/* if the nesting level is now zero, the top-level document is done */
+		if (decoder->nesting == 0) {
+			/* like kill but not an error */
+			debug("nesting is zero, document is done");
+			decoder->fd = -1;
+
+			/* return end-of-file to the caller */
+			return 0;
+		}
+	}
+
+	/* if there are unread bytes pending in the stream, discard them */
+	while (decoder->pending > 0) {
+		if (read_int8(decoder, &tbyte))
+			CODER_KILL(decoder, "read error discarding pending bytes");
+
+		decoder->pending--;
+	}
+
+	/* get the type byte */
+	if (read_int8(decoder, &tbyte))
+		CODER_KILL(decoder, "read error on type byte");
+
+	decoder->node.type = tbyte;
+	decoder->pending = 0;
+
+	debug("got type byte 0x%02x", decoder->node.type);
+
+	/* EOO is special; it has no name/data following */
+	if (decoder->node.type == BSON_EOO) {
+		decoder->node.name[0] = '\0';
+	} else {
+
+		/* get the node name */
+		nlen = 0;
+
+		for (;;) {
+			if (nlen >= BSON_MAXNAME)
+				CODER_KILL(decoder, "node name overflow");
+
+			if (read_int8(decoder, (int8_t *)&decoder->node.name[nlen]))
+				CODER_KILL(decoder, "read error on node name");
+
+			if (decoder->node.name[nlen] == '\0')
+				break;
+
+			nlen++;
+		}
+
+		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");
+			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;
+
+		case BSON_DOUBLE:
+			if (read_double(decoder, &decoder->node.d))
+				CODER_KILL(decoder, "read error on BSON_DOUBLE");
+
+			break;
+
+		case BSON_STRING:
+			if (read_int32(decoder, &decoder->pending))
+				CODER_KILL(decoder, "read error on BSON_STRING length");
+			break;
+
+		case BSON_BINDATA:
+			if (read_int32(decoder, &decoder->pending))
+				CODER_KILL(decoder, "read error on BSON_BINDATA size");
+
+			if (read_int8(decoder, &tbyte))
+				CODER_KILL(decoder, "read error on BSON_BINDATA subtype");
+
+			decoder->node.subtype = tbyte;
+			break;
+
+			/* XXX currently not supporting other types */
+		default:
+			CODER_KILL(decoder, "unsupported node type");
+		}
+	}
+
+	/* call the callback and pass its results back */
+	return decoder->callback(decoder, decoder->private, &decoder->node);
+}
+
+int
+bson_decoder_copy_data(bson_decoder_t decoder, void *buf)
+{
+	int result;
+
+	CODER_CHECK(decoder);
+
+	/* copy data */
+	result = read_x(decoder, buf, decoder->pending);
+
+	if (result != 0)
+		CODER_KILL(decoder, "read error on copy_data");
+
+	/* pending count is discharged */
+	decoder->pending = 0;
+	return 0;
+}
+
+size_t
+bson_decoder_data_pending(bson_decoder_t decoder)
+{
+	return decoder->pending;
+}
+
+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)
+{
+	return write_x(encoder, &b, sizeof(b));
+}
+
+static int
+write_int32(bson_encoder_t encoder, int32_t i)
+{
+	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)
+{
+	return write_x(encoder, &d, sizeof(d));
+}
+
+static int
+write_name(bson_encoder_t encoder, const char *name)
+{
+	size_t len = strlen(name);
+
+	if (len > BSON_MAXNAME)
+		CODER_KILL(encoder, "node name too long");
+
+	return write_x(encoder, name, len + 1);
+}
+
+int
+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");
+
+	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)
+{
+	CODER_CHECK(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;
+}
+
+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_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;
+}
+
+int
+bson_encoder_append_double(bson_encoder_t encoder, const char *name, double value)
+{
+	CODER_CHECK(encoder);
+
+	if (write_int8(encoder, BSON_DOUBLE) ||
+	    write_name(encoder, name) ||
+	    write_double(encoder, value))
+		CODER_KILL(encoder, "write error on BSON_DOUBLE");
+
+
+	return 0;
+}
+
+int
+bson_encoder_append_string(bson_encoder_t encoder, const char *name, const char *string)
+{
+	size_t len;
+
+	CODER_CHECK(encoder);
+
+	len = strlen(string) + 1;	/* include trailing nul */
+
+	if (write_int8(encoder, BSON_STRING) ||
+	    write_name(encoder, name) ||
+	    write_int32(encoder, len) ||
+	    write_x(encoder, string, len))
+		CODER_KILL(encoder, "write error on BSON_STRING");
+
+	return 0;
+}
+
+int
+bson_encoder_append_binary(bson_encoder_t encoder, const char *name, bson_binary_subtype_t subtype, size_t size, const void *data)
+{
+	CODER_CHECK(encoder);
+
+	if (write_int8(encoder, BSON_BINDATA) ||
+	    write_name(encoder, name) ||
+	    write_int32(encoder, size) ||
+	    write_int8(encoder, subtype) ||
+	    write_x(encoder, data, size))
+		CODER_KILL(encoder, "write error on BSON_BINDATA");
+
+	return 0;
+}