From c522b5446dd4e692d15b37de8ad199765259e35b Mon Sep 17 00:00:00 2001 From: px4dev Date: Sat, 27 Oct 2012 20:39:37 -0700 Subject: [PATCH] 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 + * + * 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 +#include + +#include +#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. */