From 9a85801cb02af887b89bb2a1b2f09e2f9d4fcd7e Mon Sep 17 00:00:00 2001
From: px4dev <px4@purgatory.org>
Date: Mon, 29 Oct 2012 18:01:31 -0700
Subject: [PATCH 1/9] Doxyheader fixes

---
 apps/drivers/device/cdev.cpp   | 4 +++-
 apps/drivers/device/device.cpp | 4 +++-
 apps/drivers/device/device.h   | 4 +++-
 apps/drivers/device/i2c.cpp    | 4 +++-
 apps/drivers/device/i2c.h      | 4 +++-
 apps/drivers/device/pio.cpp    | 4 +++-
 apps/drivers/device/spi.cpp    | 4 +++-
 apps/drivers/device/spi.h      | 4 +++-
 apps/drivers/drv_gpio.h        | 6 ++++--
 9 files changed, 28 insertions(+), 10 deletions(-)

diff --git a/apps/drivers/device/cdev.cpp b/apps/drivers/device/cdev.cpp
index d07d26e824..422321850a 100644
--- a/apps/drivers/device/cdev.cpp
+++ b/apps/drivers/device/cdev.cpp
@@ -32,7 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file Character device base class.
+ * @file cdev.cpp
+ *
+ * Character device base class.
  */
 
 #include "device.h"
diff --git a/apps/drivers/device/device.cpp b/apps/drivers/device/device.cpp
index c14a3234da..04a5222c36 100644
--- a/apps/drivers/device/device.cpp
+++ b/apps/drivers/device/device.cpp
@@ -32,7 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file Fundamental driver base class for the device framework.
+ * @file device.cpp
+ *
+ * Fundamental driver base class for the device framework.
  */
 
 #include "device.h"
diff --git a/apps/drivers/device/device.h b/apps/drivers/device/device.h
index 01692c3154..7d375aab91 100644
--- a/apps/drivers/device/device.h
+++ b/apps/drivers/device/device.h
@@ -32,7 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file Definitions for the generic base classes in the device framework.
+ * @file device.h
+ *
+ * Definitions for the generic base classes in the device framework.
  */
 
 #ifndef _DEVICE_DEVICE_H
diff --git a/apps/drivers/device/i2c.cpp b/apps/drivers/device/i2c.cpp
index 4b832b5485..56112d7677 100644
--- a/apps/drivers/device/i2c.cpp
+++ b/apps/drivers/device/i2c.cpp
@@ -32,7 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file Base class for devices attached via the I2C bus.
+ * @file i2c.cpp
+ *
+ * Base class for devices attached via the I2C bus.
  *
  * @todo Bus frequency changes; currently we do nothing with the value
  *       that is supplied.  Should we just depend on the bus knowing?
diff --git a/apps/drivers/device/i2c.h b/apps/drivers/device/i2c.h
index eb1b6cb053..3112e8e376 100644
--- a/apps/drivers/device/i2c.h
+++ b/apps/drivers/device/i2c.h
@@ -32,7 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file Base class for devices connected via I2C.
+ * @file i2c.h
+ *
+ * Base class for devices connected via I2C.
  */
 
 #ifndef _DEVICE_I2C_H
diff --git a/apps/drivers/device/pio.cpp b/apps/drivers/device/pio.cpp
index 5179752b55..f3a805a5e6 100644
--- a/apps/drivers/device/pio.cpp
+++ b/apps/drivers/device/pio.cpp
@@ -32,7 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file Base class for devices accessed via PIO to registers.
+ * @file pio.cpp
+ *
+ * Base class for devices accessed via PIO to registers.
  */
 
 #include "device.h"
diff --git a/apps/drivers/device/spi.cpp b/apps/drivers/device/spi.cpp
index 528333e041..63c7c12aa1 100644
--- a/apps/drivers/device/spi.cpp
+++ b/apps/drivers/device/spi.cpp
@@ -32,7 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file Base class for devices connected via SPI.
+ * @file spi.cpp
+ *
+ * Base class for devices connected via SPI.
  *
  * @todo Work out if caching the mode/frequency would save any time.
  *
diff --git a/apps/drivers/device/spi.h b/apps/drivers/device/spi.h
index e8c8e2c5e2..d2d01efb35 100644
--- a/apps/drivers/device/spi.h
+++ b/apps/drivers/device/spi.h
@@ -32,7 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file Base class for devices connected via SPI.
+ * @file spi.h
+ *
+ * Base class for devices connected via SPI.
  */
 
 #ifndef _DEVICE_SPI_H
diff --git a/apps/drivers/drv_gpio.h b/apps/drivers/drv_gpio.h
index 5b86dd920d..b8d1fa0f0a 100644
--- a/apps/drivers/drv_gpio.h
+++ b/apps/drivers/drv_gpio.h
@@ -32,7 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file	Generic GPIO ioctl interface.
+ * @file drv_gpio.h
+ *
+ * Generic GPIO ioctl interface.
  */
 
 #ifndef _DRV_GPIO_H
@@ -78,7 +80,7 @@
  * Note that there may be board-specific relationships between GPIOs;
  * applications using GPIOs should be aware of this.
  */
-#define _GPIOCBASE	0x6700
+#define _GPIOCBASE	0x2700
 #define GPIOC(_x)	_IOC(_GPIOCBASE, _x)
 
 /** reset all board GPIOs to their default state */

From 270a5d351f69676bfdd6ada4aa793953265f0491 Mon Sep 17 00:00:00 2001
From: px4dev <px4@purgatory.org>
Date: Mon, 29 Oct 2012 21:47:37 -0700
Subject: [PATCH 2/9] Warning fixes.

---
 apps/drivers/hmc5883/hmc5883.cpp | 1 +
 apps/px4/tests/test_hrt.c        | 1 +
 apps/px4/tests/test_uart_send.c  | 2 +-
 3 files changed, 3 insertions(+), 1 deletion(-)

diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp
index 6fefbfafcc..2b4fab1510 100644
--- a/apps/drivers/hmc5883/hmc5883.cpp
+++ b/apps/drivers/hmc5883/hmc5883.cpp
@@ -1045,6 +1045,7 @@ int HMC5883::check_calibration()
 		_calibrated = false;
 		// XXX Notify system via uORB
 	}
+	return 0;
 }
 
 int HMC5883::set_excitement(unsigned enable)
diff --git a/apps/px4/tests/test_hrt.c b/apps/px4/tests/test_hrt.c
index f364ea0805..3730272a24 100644
--- a/apps/px4/tests/test_hrt.c
+++ b/apps/px4/tests/test_hrt.c
@@ -39,6 +39,7 @@
 #include <nuttx/config.h>
 
 #include <sys/types.h>
+#include <sys/time.h>
 
 #include <stdio.h>
 #include <stdlib.h>
diff --git a/apps/px4/tests/test_uart_send.c b/apps/px4/tests/test_uart_send.c
index a88e617d92..f5c26e9f3b 100644
--- a/apps/px4/tests/test_uart_send.c
+++ b/apps/px4/tests/test_uart_send.c
@@ -112,7 +112,7 @@ int test_uart_send(int argc, char *argv[])
 
 	char sample_test_uart[25];// = {'S', 'A', 'M', 'P', 'L', 'E', ' ', '\n'};
 
-	int i, r, n;
+	int i, n;
 
 	uint64_t start_time = hrt_absolute_time();
 

From c522b5446dd4e692d15b37de8ad199765259e35b Mon Sep 17 00:00:00 2001
From: px4dev <px4@purgatory.org>
Date: Sat, 27 Oct 2012 20:39:37 -0700
Subject: [PATCH 3/9] Work in progress on to/from memory BSON coding.

---
 apps/px4/tests/test_bson.c     | 187 +++++++++++++++++++++++++++++++++
 apps/px4/tests/tests.h         |   1 +
 apps/px4/tests/tests_main.c    |   1 +
 apps/px4/tests/tests_param.c   |   2 -
 apps/systemlib/bson/tinybson.c | 183 +++++++++++++++++++++++++++-----
 apps/systemlib/bson/tinybson.h |  80 +++++++++++++-
 6 files changed, 423 insertions(+), 31 deletions(-)
 create mode 100644 apps/px4/tests/test_bson.c

diff --git a/apps/px4/tests/test_bson.c b/apps/px4/tests/test_bson.c
new file mode 100644
index 0000000000..4ca765e53a
--- /dev/null
+++ b/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);
+}
\ No newline at end of file
diff --git a/apps/px4/tests/tests.h b/apps/px4/tests/tests.h
index 8dc9d34e5e..1023f5f516 100644
--- a/apps/px4/tests/tests.h
+++ b/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 */
diff --git a/apps/px4/tests/tests_main.c b/apps/px4/tests/tests_main.c
index 9604710c59..b9f6835b09 100644
--- a/apps/px4/tests/tests_main.c
+++ b/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}
diff --git a/apps/px4/tests/tests_param.c b/apps/px4/tests/tests_param.c
index 5ac9f03430..88eff30f1c 100644
--- a/apps/px4/tests/tests_param.c
+++ b/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;
diff --git a/apps/systemlib/bson/tinybson.c b/apps/systemlib/bson/tinybson.c
index 75578d2ecb..10598e645c 100644
--- a/apps/systemlib/bson/tinybson.c
+++ b/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_KILL(_c, _reason)	do { debug("killed: %s", _reason); _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->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 */
-	if (decoder->pending == 0)
-		return 0;
+	/* copy data */
+	result = read_x(decoder, buf, decoder->pending);
 
-	/* copy bytes per the node size */
-	result = read(decoder->fd, buf, decoder->pending);
-
-	if (result != decoder->pending)
+	if (result != 0)
 		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(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_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
@@ -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(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");
@@ -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;
diff --git a/apps/systemlib/bson/tinybson.h b/apps/systemlib/bson/tinybson.h
index b6229dc508..833bbf6c42 100644
--- a/apps/systemlib/bson/tinybson.h
+++ b/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.
  */

From 3d750bc38c2ef5f147475cc8a54f605cbf9f772a Mon Sep 17 00:00:00 2001
From: px4dev <px4@purgatory.org>
Date: Sun, 28 Oct 2012 00:21:17 -0700
Subject: [PATCH 4/9] Build fixes

---
 apps/systemlib/bson/tinybson.c | 19 ++++++++++---------
 1 file changed, 10 insertions(+), 9 deletions(-)

diff --git a/apps/systemlib/bson/tinybson.c b/apps/systemlib/bson/tinybson.c
index 10598e645c..a28217fc42 100644
--- a/apps/systemlib/bson/tinybson.c
+++ b/apps/systemlib/bson/tinybson.c
@@ -39,6 +39,7 @@
 
 #include <unistd.h>
 #include <string.h>
+#include <stdlib.h>
 #include <err.h>
 
 #include "tinybson.h"
@@ -58,7 +59,7 @@ 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;
+		return (read(decoder->fd, p, s) == (int)s) ? 0 : -1;
 
 	if (decoder->buf != NULL) {
 		unsigned newpos = decoder->bufpos + s;
@@ -130,7 +131,7 @@ bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_
 	/* read and discard document size */
 	if (read_int32(decoder, &len))
 		CODER_KILL(decoder, "failed reading length");
-	if (len > bufsize)
+	if (len > (int)bufsize)
 		CODER_KILL(decoder, "document length larger than buffer");
 
 	/* ready for decoding */
@@ -203,7 +204,7 @@ bson_decoder_next(bson_decoder_t decoder)
 		case BSON_BOOL:
 			if (read_int8(decoder, &tbyte))
 				CODER_KILL(decoder, "read error on BSON_BOOL");
-			node->b = (tbyte != 0);
+			decoder->node.b = (tbyte != 0);
 			break;
 
 		case BSON_INT:
@@ -269,21 +270,21 @@ bson_decoder_data_pending(bson_decoder_t decoder)
 }
 
 static int
-write_x(bson_encoder_t encoder, void *p, size_t s)
+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) == s) ? 0 : -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);
+			CODER_KILL(encoder, "fixed-size buffer overflow");
 
 		int8_t *newbuf = realloc(encoder->buf, encoder->bufsize + BSON_BUF_INCREMENT);
 		if (newbuf == NULL)
-			CODER_KILL(encoder);
+			CODER_KILL(encoder, "could not grow buffer");
 
 		encoder->bufsize += BSON_BUF_INCREMENT;
 	}
@@ -385,7 +386,7 @@ bson_encoder_buf_size(bson_encoder_t encoder)
 	return encoder->bufpos;
 }
 
-int
+void *
 bson_encoder_buf_data(bson_encoder_t encoder)
 {
 	/* note, no CODER_CHECK here as the caller has to clean up dead buffers */
@@ -402,7 +403,7 @@ int bson_encoder_append_bool(bson_encoder_t encoder, const char *name, bool valu
 
 	if (write_int8(encoder, BSON_INT) ||
 	    write_name(encoder, name) ||
-	    write_int(encoder, value ? 1 : 0))
+	    write_int8(encoder, value ? 1 : 0))
 		CODER_KILL(encoder, "write error on BSON_BOOL");
 
 	return 0;

From 4c18aced369b91ce5ba14c471e1677c9a28e8707 Mon Sep 17 00:00:00 2001
From: px4dev <px4@purgatory.org>
Date: Sun, 28 Oct 2012 15:42:27 -0700
Subject: [PATCH 5/9] BSON coder unit tests, fixes arising from test failures.

---
 apps/px4/tests/test_bson.c     | 148 +++++++++++++++++++++++----------
 apps/systemlib/bson/tinybson.c |  88 +++++++++++++++-----
 apps/systemlib/bson/tinybson.h |  31 +++++--
 apps/systemlib/param/param.c   |   6 +-
 4 files changed, 197 insertions(+), 76 deletions(-)

diff --git a/apps/px4/tests/test_bson.c b/apps/px4/tests/test_bson.c
index 4ca765e53a..d03a8d332c 100644
--- a/apps/px4/tests/test_bson.c
+++ b/apps/px4/tests/test_bson.c
@@ -39,34 +39,36 @@
  */
 
 #include <stdio.h>
-#include <systemlib/err.h>
+#include <string.h>
+#include <stdlib.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 const bool sample_bool = true;
+static const int32_t sample_small_int = 123;
+static const int64_t sample_big_int = (int64_t)INT_MAX + 123LL;
+static const double sample_double = 2.5f;
+static const char *sample_string = "this is a test";
+static const uint8_t sample_data[256];
+//static const char *sample_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)
+	if (bson_encoder_append_bool(encoder, "bool1", sample_bool) != 0)
 		warnx("FAIL: encoder: append bool failed");
-	if (bson_encoder_append_int(encoder, "int1", test_int) != 0)
+	if (bson_encoder_append_int(encoder, "int1", sample_small_int) != 0)
 		warnx("FAIL: encoder: append int failed");
-	if (bson_encoder_append_double(encoder, "double1", test_double) != 0)
+	if (bson_encoder_append_int(encoder, "int2", sample_big_int) != 0)
+		warnx("FAIL: encoder: append int failed");
+	if (bson_encoder_append_double(encoder, "double1", sample_double) != 0)
 		warnx("FAIL: encoder: append double failed");
-	if (bson_encoder_append_string(encoder, "string1", test_string) != 0)
+	if (bson_encoder_append_string(encoder, "string1", sample_string) != 0)
 		warnx("FAIL: encoder: append string failed");
-	if (bson_encoder_append_binary(encoder, "data1", test_data) != 0)
+	if (bson_encoder_append_binary(encoder, "data1", BSON_BIN_BINARY, sizeof(sample_data), sample_data) != 0)
 		warnx("FAIL: encoder: append data failed");
 
 	bson_encoder_fini(encoder);
@@ -77,29 +79,70 @@ encode(bson_encoder_t encoder)
 static int
 decode_callback(bson_decoder_t decoder, void *private, bson_node_t node)
 {
+	unsigned len;
+
 	if (!strcmp(node->name, "bool1")) {
-		if (node->b != test_bool)
+		if (node->type != BSON_BOOL) {
+			warnx("FAIL: decoder: bool1 type %d, expected %d", node->type, BSON_BOOL);
+			return 1;
+		}
+		if (node->b != sample_bool) {
 			warnx("FAIL: decoder: bool1 value %s, expected %s", 
 				(node->b ? "true" : "false"),
-				(test_bool ? "true" : "false"));
+				(sample_bool ? "true" : "false"));
+			return 1;
+		}
+		warnx("PASS: decoder: bool1");
 		return 1;
 	}
 	if (!strcmp(node->name, "int1")) {
-		if (node->i != test_int)
-			warnx("FAIL: decoder: int1 value %d, expected %d", node->i, test_int);
+		if (node->type != BSON_INT32) {
+			warnx("FAIL: decoder: int1 type %d, expected %d", node->type, BSON_INT32);
+			return 1;
+		}
+		if (node->i != sample_small_int) {
+			warnx("FAIL: decoder: int1 value %lld, expected %d", node->i, sample_small_int);
+			return 1;
+		}
+		warnx("PASS: decoder: int1");
+		return 1;
+	}
+	if (!strcmp(node->name, "int2")) {
+		if (node->type != BSON_INT64) {
+			warnx("FAIL: decoder: int2 type %d, expected %d", node->type, BSON_INT64);
+			return 1;
+		}
+		if (node->i != sample_big_int) {
+			warnx("FAIL: decoder: int2 value %lld, expected %lld", node->i, sample_big_int);
+			return 1;
+		}
+		warnx("PASS: decoder: int2");
 		return 1;
 	}
 	if (!strcmp(node->name, "double1")) {
-		if (node->d != test_double)
-			warnx("FAIL: decoder: double1 value %f, expected %f", node->d, test_double);
+		if (node->type != BSON_DOUBLE) {
+			warnx("FAIL: decoder: double1 type %d, expected %d", node->type, BSON_DOUBLE);
+			return 1;
+		}
+		if (node->d != sample_double) {
+			warnx("FAIL: decoder: double1 value %f, expected %f", node->d, sample_double);
+			return 1;
+		}
+		warnx("PASS: decoder: double1");
 		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);
+		if (node->type != BSON_STRING) {
+			warnx("FAIL: decoder: string1 type %d, expected %d", node->type, BSON_STRING);
 			return 1;
+		}
+
+		len = bson_decoder_data_pending(decoder);
+
+		if (len != strlen(sample_string) + 1) {
+			warnx("FAIL: decoder: string1 length %d wrong, expected %d", len, strlen(sample_string) + 1);
+			return 1;
+		}
 
 		char sbuf[len];
 
@@ -115,20 +158,31 @@ decode_callback(bson_decoder_t decoder, void *private, bson_node_t node)
 			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);
+		if (strcmp(sbuf, sample_string)) {
+			warnx("FAIL: decoder: string1 value '%s', expected '%s'", sbuf, sample_string);
 			return 1;
 		}
+		warnx("PASS: decoder: string1");
 		return 1;
 	}
 	if (!strcmp(node->name, "data1")) {
-		unsigned len = bson_decoder_data_pending(decoder);
+		if (node->type != BSON_BINDATA) {
+			warnx("FAIL: decoder: data1 type %d, expected %d", node->type, BSON_BINDATA);
+			return 1;
+		}
 
-		if (len != sizeof(test_data)) {
-			warnx("FAIL: decoder: data1 length %d, expected %d", len, sizeof(test_data));
+		len = bson_decoder_data_pending(decoder);
+
+		if (len != sizeof(sample_data)) {
+			warnx("FAIL: decoder: data1 length %d, expected %d", len, sizeof(sample_data));
 			return 1;
 		}
 		
+		if (node->subtype != BSON_BIN_BINARY) {
+			warnx("FAIL: decoder: data1 subtype %d, expected %d", node->subtype, BSON_BIN_BINARY);
+			return 1;
+		}
+
 		uint8_t dbuf[len];
 
 		if (bson_decoder_copy_data(decoder, dbuf)) {
@@ -139,18 +193,20 @@ decode_callback(bson_decoder_t decoder, void *private, bson_node_t node)
 			warnx("FAIL: decoder: data1 copy did not exhaust all data");
 			return 1;
 		}
-		if (memcmp(test_data, dbuf, len)) {
+		if (memcmp(sample_data, dbuf, len)) {
 			warnx("FAIL: decoder: data1 compare fail");
 			return 1;
 		}
+		warnx("PASS: decoder: data1");
 		return 1;
 	}
 
-	warnx("FAIL: decoder: unexpected node name '%s'", node->name);
+	if (node->type != BSON_EOO)
+		warnx("FAIL: decoder: unexpected node name '%s'", node->name);
 	return 1;
 }
 
-static int
+static void
 decode(bson_decoder_t decoder)
 {
 	int result;
@@ -163,24 +219,26 @@ decode(bson_decoder_t decoder)
 int
 test_bson(int argc, char *argv[])
 {
-	bson_encoder_t encoder;
-	bson_decoder_t decoder;
+	struct bson_encoder_s encoder;
+	struct bson_decoder_s decoder;
 	void *buf;
-	int len, fd;
+	int len;
 
 	/* 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);
+		errx(1, "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);
+		errx(1, "FAIL: bson_encoder_buf_len");
+	buf = bson_encoder_buf_data(&encoder);
+	if (buf == NULL)
+		errx(1, "FAIL: bson_encoder_buf_data");
 
 	/* now test-decode it */
-	if (bson_decoder_init_buf(&decoder, buf, len))
-		errx("FAIL: bson_decoder_init_buf");
-	decode(decoder);
+	if (bson_decoder_init_buf(&decoder, buf, len, decode_callback, NULL))
+		errx(1, "FAIL: bson_decoder_init_buf");
+	decode(&decoder);
 	free(buf);
 
 	exit(0);
diff --git a/apps/systemlib/bson/tinybson.c b/apps/systemlib/bson/tinybson.c
index a28217fc42..e1a9324a2e 100644
--- a/apps/systemlib/bson/tinybson.c
+++ b/apps/systemlib/bson/tinybson.c
@@ -50,7 +50,7 @@
 # define debug(fmt, args...)		do { } while(0)
 #endif
 
-#define CODER_CHECK(_c)		do { if (_c->dead) 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
@@ -58,17 +58,20 @@ read_x(bson_decoder_t decoder, void *p, size_t s)
 {
 	CODER_CHECK(decoder);
 
-	if (decoder->fd > 0)
+	if (decoder->fd > -1)
 		return (read(decoder->fd, p, s) == (int)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;
-		}
+		/* 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;
 }
 
@@ -84,6 +87,12 @@ 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)
 {
@@ -106,7 +115,7 @@ bson_decoder_init_file(bson_decoder_t decoder, int fd, bson_decoder_callback cal
 
 	/* read and discard document size */
 	if (read_int32(decoder, &junk))
-		CODER_KILL(decoder, "failed reading length");
+		CODER_KILL(decoder, "failed discarding length");
 
 	/* ready for decoding */
 	return 0;
@@ -117,6 +126,10 @@ bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_
 {
 	int32_t	len;
 
+	/* argument sanity */
+	if ((buf == NULL) || (bufsize < 5) || (callback == NULL))
+		return -1;
+
 	decoder->fd = -1;
 	decoder->buf = (uint8_t *)buf;
 	decoder->dead = false;
@@ -142,6 +155,7 @@ int
 bson_decoder_next(bson_decoder_t decoder)
 {
 	int8_t	tbyte;
+	int32_t tint;
 	unsigned nlen;
 
 	CODER_CHECK(decoder);
@@ -180,7 +194,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;
@@ -207,8 +223,14 @@ bson_decoder_next(bson_decoder_t decoder)
 			decoder->node.b = (tbyte != 0);
 			break;
 
-		case BSON_INT:
-			if (read_int32(decoder, &decoder->node.i))
+		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;
@@ -222,7 +244,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:
@@ -282,15 +303,18 @@ write_x(bson_encoder_t encoder, const void *p, size_t s)
 		if (!encoder->realloc_ok)
 			CODER_KILL(encoder, "fixed-size buffer overflow");
 
-		int8_t *newbuf = realloc(encoder->buf, encoder->bufsize + BSON_BUF_INCREMENT);
+		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;
 }
@@ -307,6 +331,12 @@ 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)
 {
@@ -319,7 +349,7 @@ 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");
 
 	return write_x(encoder, name, len + 1);
 }
@@ -401,7 +431,7 @@ int bson_encoder_append_bool(bson_encoder_t encoder, const char *name, bool valu
 {
 	CODER_CHECK(encoder);
 
-	if (write_int8(encoder, BSON_INT) ||
+	if (write_int8(encoder, BSON_BOOL) ||
 	    write_name(encoder, name) ||
 	    write_int8(encoder, value ? 1 : 0))
 		CODER_KILL(encoder, "write error on BSON_BOOL");
@@ -410,13 +440,25 @@ int bson_encoder_append_bool(bson_encoder_t encoder, const char *name, bool valu
 }
 
 int
-bson_encoder_append_int(bson_encoder_t encoder, const char *name, int32_t value)
+bson_encoder_append_int(bson_encoder_t encoder, const char *name, int64_t value)
 {
+	bool result;
+
 	CODER_CHECK(encoder);
 
-	if (write_int8(encoder, BSON_INT) ||
-	    write_name(encoder, name) ||
-	    write_int32(encoder, value))
+	/* 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;
@@ -443,12 +485,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_x(encoder, name, len + 1))
+	    write_x(encoder, string, len))
 		CODER_KILL(encoder, "write error on BSON_STRING");
 
 	return 0;
diff --git a/apps/systemlib/bson/tinybson.h b/apps/systemlib/bson/tinybson.h
index 833bbf6c42..d820aa7b90 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 {
@@ -87,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;
 	};
@@ -225,26 +224,48 @@ __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);
 
diff --git a/apps/systemlib/param/param.c b/apps/systemlib/param/param.c
index eeb867f11e..44f05047a2 100644
--- a/apps/systemlib/param/param.c
+++ b/apps/systemlib/param/param.c
@@ -452,7 +452,7 @@ param_reset(param_t param)
 
 		/* if we found one, erase it */
 		if (s != NULL) {
-			int pos = utarry_eltidx(param_values, s);
+			int pos = utarray_eltidx(param_values, s);
 			utarray_erase(param_values, pos, 1);
 		}
 	}
@@ -489,7 +489,7 @@ param_export(int fd, bool only_unsaved)
 
 	param_lock();
 
-	bson_encoder_init(&encoder, fd);
+	bson_encoder_init_file(&encoder, fd);
 
 	/* no modified parameters -> we are done */
 	if (param_values == NULL) {
@@ -680,7 +680,7 @@ param_import_internal(int fd, bool mark_saved)
 	int result = -1;
 	struct param_import_state state;
 
-	if (bson_decoder_init(&decoder, fd, param_import_callback, &state)) {
+	if (bson_decoder_init_file(&decoder, fd, param_import_callback, &state)) {
 		debug("decoder init failed");
 		goto out;
 	}

From 3420e7b828894bff502cbf9f70fac5ce262542b2 Mon Sep 17 00:00:00 2001
From: px4dev <px4@purgatory.org>
Date: Sun, 28 Oct 2012 16:04:25 -0700
Subject: [PATCH 6/9] Fix param handling of 32-bit BSON nodes

---
 apps/systemlib/param/param.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/apps/systemlib/param/param.c b/apps/systemlib/param/param.c
index 44f05047a2..365bd3d199 100644
--- a/apps/systemlib/param/param.c
+++ b/apps/systemlib/param/param.c
@@ -600,7 +600,7 @@ param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node)
 	 */
 
 	switch (node->type) {
-	case BSON_INT:
+	case BSON_INT32:
 		if (param_type(param) != PARAM_TYPE_INT32) {
 			debug("unexpected type for '%s", node->name);
 			goto out;

From 7203ba797e40b146e7b85b83a6f691e260245a58 Mon Sep 17 00:00:00 2001
From: px4dev <px4@purgatory.org>
Date: Mon, 29 Oct 2012 18:00:32 -0700
Subject: [PATCH 7/9] bson-based boardinfo working

---
 apps/systemcmds/boardinfo/boardinfo.c | 234 +++++++++++++++++++++++++-
 apps/systemlib/bson/tinybson.c        |   2 +-
 2 files changed, 233 insertions(+), 3 deletions(-)

diff --git a/apps/systemcmds/boardinfo/boardinfo.c b/apps/systemcmds/boardinfo/boardinfo.c
index bce8c27e7c..9e6189fec1 100644
--- a/apps/systemcmds/boardinfo/boardinfo.c
+++ b/apps/systemcmds/boardinfo/boardinfo.c
@@ -37,17 +37,246 @@
  * autopilot and carrier board information app
  */
 
-
 #include <nuttx/config.h>
 #include <unistd.h>
 #include <stdio.h>
 #include <string.h>
 #include <stdlib.h>
 
-#include "systemlib/systemlib.h"
+#include <nuttx/i2c.h>
+
+#include <systemlib/systemlib.h>
+#include <systemlib/err.h>
+#include <systemlib/bson/tinybson.h>
 
 __EXPORT int boardinfo_main(int argc, char *argv[]);
 
+#if 1
+
+struct eeprom_info_s
+{
+	unsigned	bus;
+	unsigned	address;
+	unsigned	page_size;
+	unsigned	page_count;
+	unsigned	page_write_delay;
+};
+
+/* XXX currently code below only supports 8-bit addressing */
+const struct eeprom_info_s eeprom_info[] = {
+	{3, 0x57, 8, 16, 3300},
+	{0, 0, 0, 0, 0}
+};
+
+struct board_parameter_s {
+	const char	*name;
+	bson_type_t	type;
+};
+
+const struct board_parameter_s board_parameters[] = {
+	{"name",	BSON_STRING},	/* ascii board name */
+	{"vers",	BSON_INT32},	/* board version (major << 8) | minor */
+	{"date",	BSON_INT32},	/* manufacture date */
+	{"build",	BSON_INT32}	/* build code (fab << 8) | tester */
+};
+
+const unsigned num_parameters = sizeof(board_parameters) / sizeof(board_parameters[0]);
+
+static int
+eeprom_write(const struct eeprom_info_s *eeprom, uint8_t *buf, unsigned size)
+{
+	int result = -1;
+
+	struct i2c_dev_s *dev = up_i2cinitialize(eeprom->bus);
+	if (dev == NULL) {
+		warnx("failed to init bus %d for EEPROM", eeprom->bus);
+		goto out;
+	}
+	I2C_SETFREQUENCY(dev, 400000);
+
+	/* loop until all data has been transferred */
+	for (unsigned address = 0; address < size; ) {
+
+		uint8_t pagebuf[eeprom->page_size + 1];
+
+		/* how many bytes available to transfer? */
+		unsigned count = size - address;
+
+		/* constrain writes to the page size */
+		if (count > eeprom->page_size)
+			count = eeprom->page_size;
+
+		pagebuf[0] = address & 0xff;
+		memcpy(pagebuf + 1, buf + address, count);
+
+		struct i2c_msg_s msgv[1] = {
+			{
+				.addr = eeprom->address,
+				.flags = 0,
+				.buffer = pagebuf,
+				.length = count + 1
+			}
+		};
+
+		warnx("write 0x%02x/%u", address, count);
+		result = I2C_TRANSFER(dev, msgv, 1);
+		if (result != OK) {
+			warnx("EEPROM write failed: %d", result);
+			goto out;
+		}
+		usleep(eeprom->page_write_delay);
+		address += count;
+	}
+
+out:
+	if (dev != NULL)
+		up_i2cuninitialize(dev);
+	return result;
+}
+
+static int
+eeprom_read(const struct eeprom_info_s *eeprom, uint8_t *buf, unsigned size)
+{
+	int result = -1;
+
+	struct i2c_dev_s *dev = up_i2cinitialize(eeprom->bus);
+	if (dev == NULL) {
+		warnx("failed to init bus %d for EEPROM", eeprom->bus);
+		goto out;
+	}
+	I2C_SETFREQUENCY(dev, 400000);
+
+	/* loop until all data has been transferred */
+	for (unsigned address = 0; address < size; ) {
+
+		/* how many bytes available to transfer? */
+		unsigned count = size - address;
+
+		/* constrain transfers to the page size (bus anti-hog) */
+		if (count > eeprom->page_size)
+			count = eeprom->page_size;
+
+		uint8_t addr = address;
+		struct i2c_msg_s msgv[2] = {
+			{
+				.addr = eeprom->address,
+				.flags = 0,
+				.buffer = &addr,
+				.length = 1
+			},
+			{
+				.addr = eeprom->address,
+				.flags = I2C_M_READ,
+				.buffer = buf + address,
+				.length = count
+			}
+		};
+
+		warnx("read 0x%02x/%u", address, count);
+		result = I2C_TRANSFER(dev, msgv, 2);
+		if (result != OK) {
+			warnx("EEPROM read failed: %d", result);
+			goto out;
+		}
+		address += count;
+	}
+
+out:
+	if (dev != NULL)
+		up_i2cuninitialize(dev);
+	return result;
+}
+
+static void
+boardinfo_set(const struct eeprom_info_s *eeprom, char *spec)
+{
+	struct bson_encoder_s encoder;
+	int result = 1;
+	char *state, *token;
+	unsigned i;
+
+	/* create the encoder and make a writable copy of the spec */
+	bson_encoder_init_buf(&encoder, NULL, 0);
+
+	for (i = 0, token = strtok_r(spec, ",", &state);
+	     token && (i < num_parameters);
+	     i++, token = strtok_r(NULL, ",", &state)) {
+
+		switch (board_parameters[i].type) {
+		case BSON_STRING:
+			result = bson_encoder_append_string(&encoder, board_parameters[i].name, token);
+			break;
+		case BSON_INT32:
+			result = bson_encoder_append_int(&encoder, board_parameters[i].name, strtoul(token, NULL, 0));
+			break;
+		default:
+			result = 1;
+		}
+		if (result) {
+			warnx("bson append failed for %s<%s>", board_parameters[i].name, token);
+			goto out;
+		}
+	}
+	bson_encoder_fini(&encoder);
+	if (i != num_parameters) {
+		warnx("incorrect parameter list, expected: \"<name>,<version><date>,<buildcode>\"");
+		result = 1;
+		goto out;
+	}
+	if (bson_encoder_buf_size(&encoder) > (int)(eeprom->page_size * eeprom->page_count)) {
+		warnx("data too large for EEPROM");
+		result = 1;
+		goto out;
+	}
+	if (*(uint32_t *)bson_encoder_buf_data(&encoder) != bson_encoder_buf_size(&encoder)) {
+		warnx("buffer length mismatch");
+		result = 1;
+		goto out;
+	}
+	warnx("writing %p/%u", bson_encoder_buf_data(&encoder), bson_encoder_buf_size(&encoder));
+
+	result = eeprom_write(eeprom, (uint8_t *)bson_encoder_buf_data(&encoder), bson_encoder_buf_size(&encoder));
+	if (result < 0) {
+		warnc(-result, "error writing EEPROM");
+		result = 1;
+	} else {
+		result = 0;
+	}
+
+out:
+	free(bson_encoder_buf_data(&encoder));
+
+	exit(result);
+}
+
+static void
+boardinfo_show(const struct eeprom_info_s *eeprom)
+{
+	uint32_t size = 0xffffffff;
+	int result;
+
+	result = eeprom_read(eeprom, (uint8_t *)&size, sizeof(size));
+	if (result != 0) {
+		warnx("failed reading ID ROM length");
+	}
+	warnx("data length 0x%08x", size);
+
+	exit(0);
+}
+
+int
+boardinfo_main(int argc, char *argv[])
+{
+	if (!strcmp(argv[1], "set"))
+		boardinfo_set(&eeprom_info[0], argv[2]);
+
+	if (!strcmp(argv[1], "show"))
+		boardinfo_show(&eeprom_info[0]);
+
+	errx(1, "missing/unrecognised command, try one of 'set', 'show', 'test'");
+}
+
+#else
 /**
  * Reads out the board information
  *
@@ -265,3 +494,4 @@ int boardinfo_main(int argc, char *argv[])
 }
 
 
+#endif
\ No newline at end of file
diff --git a/apps/systemlib/bson/tinybson.c b/apps/systemlib/bson/tinybson.c
index e1a9324a2e..1b5a1e1eec 100644
--- a/apps/systemlib/bson/tinybson.c
+++ b/apps/systemlib/bson/tinybson.c
@@ -144,7 +144,7 @@ bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_
 	/* read and discard document size */
 	if (read_int32(decoder, &len))
 		CODER_KILL(decoder, "failed reading length");
-	if (len > (int)bufsize)
+	if ((len > 0) && (len > (int)bufsize))
 		CODER_KILL(decoder, "document length larger than buffer");
 
 	/* ready for decoding */

From 0616d5834039cc08057b862f80f8129a7b4948af Mon Sep 17 00:00:00 2001
From: px4dev <px4@purgatory.org>
Date: Mon, 29 Oct 2012 21:46:56 -0700
Subject: [PATCH 8/9] Add 'show' and 'test' verbs to the boardinfo command. 
 Teach rcS how to use the new version.

---
 ROMFS/scripts/rcS                     |   4 +-
 apps/systemcmds/boardinfo/boardinfo.c | 156 ++++++++++++++++++++++++--
 apps/systemlib/bson/tinybson.c        |  11 +-
 apps/systemlib/bson/tinybson.h        |   4 +-
 4 files changed, 159 insertions(+), 16 deletions(-)

diff --git a/ROMFS/scripts/rcS b/ROMFS/scripts/rcS
index b5fbfe0f51..4152494e0e 100755
--- a/ROMFS/scripts/rcS
+++ b/ROMFS/scripts/rcS
@@ -83,7 +83,7 @@ else
 	#
 	# Are we attached to a PX4IOAR (AR.Drone carrier board)?
 	#
-	if boardinfo -t 7
+	if boardinfo test name PX4IOAR
 	then
 		set BOARD PX4IOAR
 		if [ -f /etc/init.d/rc.PX4IOAR ]
@@ -99,7 +99,7 @@ else
 	#
 	# Are we attached to a PX4IO?
 	#
-	if boardinfo -t 6
+	if boardinfo test name PX4IO
 	then
 		set BOARD PX4IO
 		if [ -f /etc/init.d/rc.PX4IO ]
diff --git a/apps/systemcmds/boardinfo/boardinfo.c b/apps/systemcmds/boardinfo/boardinfo.c
index 9e6189fec1..fb9c0b3a4c 100644
--- a/apps/systemcmds/boardinfo/boardinfo.c
+++ b/apps/systemcmds/boardinfo/boardinfo.c
@@ -118,7 +118,6 @@ eeprom_write(const struct eeprom_info_s *eeprom, uint8_t *buf, unsigned size)
 			}
 		};
 
-		warnx("write 0x%02x/%u", address, count);
 		result = I2C_TRANSFER(dev, msgv, 1);
 		if (result != OK) {
 			warnx("EEPROM write failed: %d", result);
@@ -172,7 +171,6 @@ eeprom_read(const struct eeprom_info_s *eeprom, uint8_t *buf, unsigned size)
 			}
 		};
 
-		warnx("read 0x%02x/%u", address, count);
 		result = I2C_TRANSFER(dev, msgv, 2);
 		if (result != OK) {
 			warnx("EEPROM read failed: %d", result);
@@ -187,6 +185,41 @@ out:
 	return result;
 }
 
+static void *
+idrom_read(const struct eeprom_info_s *eeprom)
+{
+	uint32_t size = 0xffffffff;
+	int result;
+	void *buf = NULL;
+
+	result = eeprom_read(eeprom, (uint8_t *)&size, sizeof(size));
+	if (result != 0) {
+		warnx("failed reading ID ROM length");
+		goto fail;
+	}
+	if (size > (eeprom->page_size * eeprom->page_count)) {
+		warnx("ID ROM not programmed");
+		goto fail;
+	}
+
+	buf = malloc(size);
+	if (buf == NULL) {
+		warnx("could not allocate %d bytes for ID ROM", size);
+		goto fail;
+	}
+	result = eeprom_read(eeprom, buf, size);
+	if (result != 0) {
+		warnx("failed reading ID ROM");
+		goto fail;
+	}
+	return buf;
+
+fail:
+	if (buf != NULL)
+		free(buf);
+	return NULL;
+}
+
 static void
 boardinfo_set(const struct eeprom_info_s *eeprom, char *spec)
 {
@@ -228,7 +261,7 @@ boardinfo_set(const struct eeprom_info_s *eeprom, char *spec)
 		result = 1;
 		goto out;
 	}
-	if (*(uint32_t *)bson_encoder_buf_data(&encoder) != bson_encoder_buf_size(&encoder)) {
+	if ((int)*(uint32_t *)bson_encoder_buf_data(&encoder) != bson_encoder_buf_size(&encoder)) {
 		warnx("buffer length mismatch");
 		result = 1;
 		goto out;
@@ -249,21 +282,121 @@ out:
 	exit(result);
 }
 
+static int
+boardinfo_print(bson_decoder_t decoder, void *private, bson_node_t node)
+{
+	switch (node->type) {
+	case BSON_INT32:
+		printf("%s: %d / 0x%08x\n", node->name, (int)node->i, (unsigned)node->i);
+		break;
+	case BSON_STRING: {
+		char buf[bson_decoder_data_pending(decoder)];
+		bson_decoder_copy_data(decoder, buf);
+		printf("%s: %s\n", node->name, buf);
+		break;
+	}
+	case BSON_EOO:
+		break;
+	default:
+		warnx("unexpected node type %d", node->type);
+		break;
+	}
+	return 1;
+}
+
 static void
 boardinfo_show(const struct eeprom_info_s *eeprom)
 {
-	uint32_t size = 0xffffffff;
-	int result;
+	struct bson_decoder_s decoder;
+	void *buf;
 
-	result = eeprom_read(eeprom, (uint8_t *)&size, sizeof(size));
-	if (result != 0) {
-		warnx("failed reading ID ROM length");
+	buf = idrom_read(eeprom);
+	if (buf == NULL)
+		errx(1, "ID ROM read failed");
+
+	if (bson_decoder_init_buf(&decoder, buf, 0, boardinfo_print, NULL) == 0) {
+		while (bson_decoder_next(&decoder) > 0)
+			;
+	} else {
+		warnx("failed to init decoder");
 	}
-	warnx("data length 0x%08x", size);
-
+	free(buf);
 	exit(0);
 }
 
+struct {
+	const char *property;
+	const char *value;
+} test_args;
+
+static int
+boardinfo_test_callback(bson_decoder_t decoder, void *private, bson_node_t node)
+{
+	/* reject nodes with non-matching names */
+	if (strcmp(node->name, test_args.property))
+		return 1;
+
+	/* compare node values to check for a match */
+	switch (node->type) {
+	case BSON_STRING: {
+		char buf[bson_decoder_data_pending(decoder)];
+		bson_decoder_copy_data(decoder, buf);
+
+		/* check for a match */
+		if (!strcmp(test_args.value, buf)) {
+			return 2;
+		}
+		break;
+	}
+
+	case BSON_INT32: {
+		int32_t val = strtol(test_args.value, NULL, 0);
+
+		/* check for a match */
+		if (node->i == val) {
+			return 2;
+		}
+		break;
+	}
+
+	default:
+		break;
+	}
+
+	return 1;
+}
+
+static void
+boardinfo_test(const struct eeprom_info_s *eeprom, const char *property, const char *value)
+{
+	struct bson_decoder_s decoder;
+	void *buf;
+	int result = -1;
+
+	if ((property == NULL) || (strlen(property) == 0) ||
+	    (value == NULL) || (strlen(value) == 0))
+		errx(1, "missing property name or value");
+
+	test_args.property = property;
+	test_args.value = value;
+
+	buf = idrom_read(eeprom);
+	if (buf == NULL)
+		errx(1, "ID ROM read failed");
+
+	if (bson_decoder_init_buf(&decoder, buf, 0, boardinfo_test_callback, NULL) == 0) {
+		do {
+			result = bson_decoder_next(&decoder);
+		} while (result == 1);
+	} else {
+		warnx("failed to init decoder");
+	}
+	free(buf);
+
+	/* if we matched, we exit with zero success */	
+	exit((result == 2) ? 0 : 1);
+}
+
 int
 boardinfo_main(int argc, char *argv[])
 {
@@ -273,6 +406,9 @@ boardinfo_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "show"))
 		boardinfo_show(&eeprom_info[0]);
 
+	if (!strcmp(argv[1], "test"))
+		boardinfo_test(&eeprom_info[0], argv[2], argv[3]);
+
 	errx(1, "missing/unrecognised command, try one of 'set', 'show', 'test'");
 }
 
diff --git a/apps/systemlib/bson/tinybson.c b/apps/systemlib/bson/tinybson.c
index 1b5a1e1eec..321466f87a 100644
--- a/apps/systemlib/bson/tinybson.c
+++ b/apps/systemlib/bson/tinybson.c
@@ -127,13 +127,18 @@ bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_
 	int32_t	len;
 
 	/* argument sanity */
-	if ((buf == NULL) || (bufsize < 5) || (callback == NULL))
+	if ((buf == NULL) || (callback == NULL))
 		return -1;
 
 	decoder->fd = -1;
 	decoder->buf = (uint8_t *)buf;
 	decoder->dead = false;
-	decoder->bufsize = bufsize;
+	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;
@@ -144,7 +149,7 @@ bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_
 	/* read and discard document size */
 	if (read_int32(decoder, &len))
 		CODER_KILL(decoder, "failed reading length");
-	if ((len > 0) && (len > (int)bufsize))
+	if ((len > 0) && (len > (int)decoder->bufsize))
 		CODER_KILL(decoder, "document length larger than buffer");
 
 	/* ready for decoding */
diff --git a/apps/systemlib/bson/tinybson.h b/apps/systemlib/bson/tinybson.h
index d820aa7b90..666f8191aa 100644
--- a/apps/systemlib/bson/tinybson.h
+++ b/apps/systemlib/bson/tinybson.h
@@ -134,7 +134,9 @@ __EXPORT int bson_decoder_init_file(bson_decoder_t decoder, int fd, bson_decoder
  *
  * @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 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.

From 66da4395b4b0c6843e20ab09e105756338803540 Mon Sep 17 00:00:00 2001
From: px4dev <px4@purgatory.org>
Date: Mon, 29 Oct 2012 23:07:15 -0700
Subject: [PATCH 9/9] Kill the old board info code.

---
 apps/systemcmds/boardinfo/boardinfo.c | 223 --------------------------
 apps/systemlib/systemlib.c            |  73 ---------
 apps/systemlib/systemlib.h            |   5 -
 3 files changed, 301 deletions(-)

diff --git a/apps/systemcmds/boardinfo/boardinfo.c b/apps/systemcmds/boardinfo/boardinfo.c
index fb9c0b3a4c..2328ebdb26 100644
--- a/apps/systemcmds/boardinfo/boardinfo.c
+++ b/apps/systemcmds/boardinfo/boardinfo.c
@@ -51,8 +51,6 @@
 
 __EXPORT int boardinfo_main(int argc, char *argv[]);
 
-#if 1
-
 struct eeprom_info_s
 {
 	unsigned	bus;
@@ -65,7 +63,6 @@ struct eeprom_info_s
 /* XXX currently code below only supports 8-bit addressing */
 const struct eeprom_info_s eeprom_info[] = {
 	{3, 0x57, 8, 16, 3300},
-	{0, 0, 0, 0, 0}
 };
 
 struct board_parameter_s {
@@ -411,223 +408,3 @@ boardinfo_main(int argc, char *argv[])
 
 	errx(1, "missing/unrecognised command, try one of 'set', 'show', 'test'");
 }
-
-#else
-/**
- * Reads out the board information
- *
- * @param argc the number of string arguments (including the executable name)
- * @param argv the argument strings
- *
- * @return 0 on success, 1 on error
- */
-int boardinfo_main(int argc, char *argv[])
-{
-	const char *commandline_usage = "\tusage: boardinfo [-c|-f] [-t id] [-w \"<info>\"]\n";
-
-	bool carrier_mode = false;
-	bool fmu_mode = false;
-	bool write_mode = false;
-	char *write_string = 0;
-	bool silent = false;
-	bool test_enabled = false;
-	int test_boardid = -1;
-	int ch;
-
-	while ((ch = getopt(argc, argv, "cft:w:v")) != -1) {
-		switch (ch) {
-		case 'c':
-			carrier_mode = true;
-			break;
-
-		case 'f':
-			fmu_mode = true;
-			break;
-
-		case 't':
-			test_enabled = true;
-			test_boardid = strtol(optarg, NULL, 10);
-			silent = true;
-			break;
-
-		case 'w':
-			write_mode = true;
-			write_string = optarg;
-			break;
-
-		default:
-			printf(commandline_usage);
-			exit(0);
-		}
-	}
-
-	/* Check if write is required - only one mode is allowed then */
-	if (write_mode && fmu_mode && carrier_mode) {
-		fprintf(stderr, "[boardinfo] Please choose only one mode for writing: --carrier or --fmu\n");
-		printf(commandline_usage);
-		return ERROR;
-	}
-
-	/* Write FMU information
-	if (fmu_mode && write_mode) {
-		struct fmu_board_info_s info;
-		int ret = fmu_store_board_info(&info);
-
-
-		if (ret == OK) {
-			printf("[boardinfo] Successfully wrote FMU board info\n");
-		} else {
-			fprintf(stderr, "[boardinfo] ERROR writing board info to FMU EEPROM, aborting\n");
-			return ERROR;
-		}
-	}*/
-
-	/* write carrier board info */
-	if (carrier_mode && write_mode) {
-
-		struct carrier_board_info_s info;
-		bool parse_ok = true;
-		unsigned parse_idx = 0;
-		//int maxlen = strlen(write_string);
-		char *curr_char;
-
-		/* Parse board info string */
-		if (write_string[0] != 'P' || write_string[1] != 'X' || write_string[2] != '4') {
-			fprintf(stderr, "[boardinfo] header must start with 'PX4'\n");
-			parse_ok = false;
-		}
-
-		info.header[0] = 'P'; info.header[1] = 'X'; info.header[2] = '4';
-		parse_idx = 3;
-		/* Copy board name */
-
-		int i = 0;
-
-		while (write_string[parse_idx] != 0x20 && (parse_idx < sizeof(info.board_name) + sizeof(info.header))) {
-			info.board_name[i] = write_string[parse_idx];
-			i++; parse_idx++;
-		}
-
-		/* Enforce null termination */
-		info.board_name[sizeof(info.board_name) - 1] = '\0';
-
-		curr_char = write_string + parse_idx;
-
-		/* Index is now on next field */
-		info.board_id = strtol(curr_char, &curr_char, 10);//atoi(write_string + parse_index);
-		info.board_version = strtol(curr_char, &curr_char, 10);
-
-		/* Read in multi ports */
-		for (i = 0; i < MULT_COUNT; i++) {
-			info.multi_port_config[i] = strtol(curr_char, &curr_char, 10);
-		}
-
-		/* Read in production data */
-		info.production_year = strtol(curr_char, &curr_char, 10);
-
-		if (info.production_year < 2012 || info.production_year > 3000) {
-			fprintf(stderr, "[boardinfo] production year is invalid: %d\n", info.production_year);
-			parse_ok = false;
-		}
-
-		info.production_month = strtol(curr_char, &curr_char, 10);
-
-		if (info.production_month < 1 || info.production_month > 12) {
-			fprintf(stderr, "[boardinfo] production month is invalid: %d\n", info.production_month);
-			parse_ok = false;
-		}
-
-		info.production_day = strtol(curr_char, &curr_char, 10);
-
-		if (info.production_day < 1 || info.production_day > 31) {
-			fprintf(stderr, "[boardinfo] production day is invalid: %d\n", info.production_day);
-			parse_ok = false;
-		}
-
-		info.production_fab = strtol(curr_char, &curr_char, 10);
-		info.production_tester = strtol(curr_char, &curr_char, 10);
-
-		if (!parse_ok) {
-			/* Parsing failed */
-			fprintf(stderr, "[boardinfo] failed parsing info string:\n\t%s\naborting\n", write_string);
-			return ERROR;
-
-		} else {
-			int ret = carrier_store_board_info(&info);
-
-			/* Display result */
-			if (ret == sizeof(info)) {
-				printf("[boardinfo] Successfully wrote carrier board info\n");
-
-			} else {
-				fprintf(stderr, "[boardinfo] ERROR writing board info to carrier EEPROM (forgot to pull the WRITE_ENABLE line high?), aborting\n");
-				return ERROR;
-			}
-		}
-	}
-
-	/* Print FMU information */
-	if (fmu_mode && !silent) {
-		struct fmu_board_info_s info;
-		int ret = fmu_get_board_info(&info);
-
-		/* Print human readable name */
-		if (ret == sizeof(info)) {
-			printf("[boardinfo] Autopilot:\n\t%s\n", info.header);
-
-		} else {
-			fprintf(stderr, "[boardinfo] ERROR loading board info from FMU, aborting\n");
-			return ERROR;
-		}
-	}
-
-	/* print carrier information */
-	if (carrier_mode && !silent) {
-
-		struct carrier_board_info_s info;
-		int ret = carrier_get_board_info(&info);
-
-		/* Print human readable name */
-		if (ret == sizeof(info)) {
-			printf("[boardinfo] Carrier board:\n\t%s\n", info.header);
-			printf("\tboard id:\t\t%d\n", info.board_id);
-			printf("\tversion:\t\t%d\n", info.board_version);
-
-			for (unsigned i = 0; i < MULT_COUNT; i++) {
-				printf("\tmulti port #%d:\t\t%s: function #%d\n", i, multiport_info.port_names[i], info.multi_port_config[i]);
-			}
-
-			printf("\tproduction date:\t%d-%d-%d (fab #%d / tester #%d)\n", info.production_year, info.production_month, info.production_day, info.production_fab, info.production_tester);
-
-		} else {
-			fprintf(stderr, "[boardinfo] ERROR loading board info from carrier EEPROM (errno #%d), aborting\n", -ret);
-			return ERROR;
-		}
-	}
-
-	/* test for a specific carrier */
-	if (test_enabled) {
-
-		struct carrier_board_info_s info;
-		int ret = carrier_get_board_info(&info);
-
-		if (ret != sizeof(info)) {
-			fprintf(stderr, "[boardinfo] no EEPROM for board %d\n", test_boardid);
-			exit(1);
-		}
-
-		if (info.board_id == test_boardid) {
-			printf("[boardinfo] Found carrier board with ID %d, test succeeded\n", info.board_id);
-			exit(0);
-
-		} else {
-			/* exit silently with an error so we can test for multiple boards quietly */
-			exit(1);
-		}
-	}
-
-	return 0;
-}
-
-
-#endif
\ No newline at end of file
diff --git a/apps/systemlib/systemlib.c b/apps/systemlib/systemlib.c
index 750e783f55..b596b0f0e2 100644
--- a/apps/systemlib/systemlib.c
+++ b/apps/systemlib/systemlib.c
@@ -110,76 +110,3 @@ int task_spawn(const char *name, int scheduler, int priority, int stack_size, ma
 
 	return pid;
 }
-
-#define PX4_BOARD_ID_FMU (5)
-
-int fmu_get_board_info(struct fmu_board_info_s *info)
-{
-	/* Check which FMU version we're on */
-	struct stat sb;
-	int statres;
-
-	/* Copy version-specific fields */
-	statres = stat("/dev/bma180", &sb);
-
-	if (statres == OK) {
-		/* BMA180 indicates a v1.5-v1.6 board */
-		strcpy(info->board_name, "FMU v1.6");
-		info->board_version = 16;
-
-	} else {
-		statres = stat("/dev/accel", &sb);
-
-		if (statres == OK) {
-			/* MPU-6000 indicates a v1.7+ board */
-			strcpy(info->board_name, "FMU v1.7");
-			info->board_version = 17;
-
-		} else {
-			/* If no BMA and no MPU is present, it is a v1.3 board */
-			strcpy(info->board_name, "FMU v1.3");
-			info->board_version = 13;
-		}
-	}
-
-	/* Copy general FMU fields */
-	memcpy(info->header, "PX4", 3);
-	info->board_id = PX4_BOARD_ID_FMU;
-
-	return sizeof(struct fmu_board_info_s);
-}
-
-int carrier_store_board_info(const struct carrier_board_info_s *info)
-{
-	int ret;
-	int fd = open("/dev/eeprom", O_RDWR | O_NONBLOCK);
-
-	if (fd < 0) fprintf(stderr, "[boardinfo carrier] ERROR opening carrier eeprom\n");
-
-	/* Enforce correct header */
-	ret = write(fd, info, sizeof(struct carrier_board_info_s));
-	//ret = write(fd, "PX4", 3);
-	close(fd);
-
-	return ret;
-}
-
-int carrier_get_board_info(struct carrier_board_info_s *info)
-{
-	int ret;
-	int fd = open("/dev/eeprom", O_RDONLY | O_NONBLOCK);
-
-	if (fd < 0)
-		return -1;	/* no board */
-
-	ret = read(fd, info, sizeof(struct carrier_board_info_s));
-
-	/* Enforce NUL termination of human-readable string */
-	if (ret == sizeof(struct carrier_board_info_s)) {
-		info->board_name[sizeof(info->board_name) - 1] = '\0';
-	}
-
-	close(fd);
-
-	return ret;
-}
diff --git a/apps/systemlib/systemlib.h b/apps/systemlib/systemlib.h
index f31c5cd1f9..2c53c648b7 100644
--- a/apps/systemlib/systemlib.h
+++ b/apps/systemlib/systemlib.h
@@ -117,11 +117,6 @@ struct __multiport_info {
 };
 __EXPORT extern const struct __multiport_info multiport_info;
 
-__EXPORT int carrier_store_board_info(const struct carrier_board_info_s *info);
-__EXPORT int carrier_get_board_info(struct carrier_board_info_s *info);
-
-__EXPORT int fmu_get_board_info(struct fmu_board_info_s *info);
-
 __END_DECLS
 
 #endif /* SYSTEMLIB_H_ */