From 13fc6703862862f4263d8d5d085b7a16b87190e1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 28 Apr 2013 09:54:11 +0200 Subject: Moved last libs, drivers and headers, cleaned up IO build --- src/modules/systemlib/bson/tinybson.c | 517 ++++++++++++++++++++++++++++++++++ 1 file changed, 517 insertions(+) create mode 100644 src/modules/systemlib/bson/tinybson.c (limited to 'src/modules/systemlib/bson/tinybson.c') diff --git a/src/modules/systemlib/bson/tinybson.c b/src/modules/systemlib/bson/tinybson.c new file mode 100644 index 000000000..8aca6a25d --- /dev/null +++ b/src/modules/systemlib/bson/tinybson.c @@ -0,0 +1,517 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file tinybson.c + * + * A simple subset SAX-style BSON parser and generator. + */ + +#include +#include +#include +#include + +#include "tinybson.h" + +#if 0 +# define debug(fmt, args...) do { warnx("BSON: " fmt, ##args); } while(0) +#else +# define debug(fmt, args...) do { } while(0) +#endif + +#define CODER_CHECK(_c) do { if (_c->dead) { debug("coder dead"); return -1; }} while(0) +#define CODER_KILL(_c, _reason) do { debug("killed: %s", _reason); _c->dead = true; return -1; } while(0) + +static int +read_x(bson_decoder_t decoder, void *p, size_t s) +{ + CODER_CHECK(decoder); + + if (decoder->fd > -1) + return (read(decoder->fd, p, s) == (int)s) ? 0 : -1; + + if (decoder->buf != NULL) { + /* staged operations to avoid integer overflow for corrupt data */ + if (s >= decoder->bufsize) + CODER_KILL(decoder, "buffer too small for read"); + if ((decoder->bufsize - s) < decoder->bufpos) + CODER_KILL(decoder, "not enough data for read"); + memcpy(p, (decoder->buf + decoder->bufpos), s); + decoder->bufpos += s; + return 0; + } + debug("no source"); + return -1; +} + +static int +read_int8(bson_decoder_t decoder, int8_t *b) +{ + return read_x(decoder, b, sizeof(*b)); +} + +static int +read_int32(bson_decoder_t decoder, int32_t *i) +{ + return read_x(decoder, i, sizeof(*i)); +} + +static int +read_int64(bson_decoder_t decoder, int64_t *i) +{ + return read_x(decoder, i, sizeof(*i)); +} + +static int +read_double(bson_decoder_t decoder, double *d) +{ + return read_x(decoder, d, sizeof(*d)); +} + +int +bson_decoder_init_file(bson_decoder_t decoder, int fd, bson_decoder_callback callback, void *private) +{ + int32_t junk; + + decoder->fd = fd; + decoder->buf = NULL; + decoder->dead = false; + decoder->callback = callback; + decoder->private = private; + decoder->nesting = 1; + decoder->pending = 0; + decoder->node.type = BSON_UNDEFINED; + + /* read and discard document size */ + if (read_int32(decoder, &junk)) + CODER_KILL(decoder, "failed discarding length"); + + /* ready for decoding */ + return 0; +} + +int +bson_decoder_init_buf(bson_decoder_t decoder, void *buf, unsigned bufsize, bson_decoder_callback callback, void *private) +{ + int32_t len; + + /* argument sanity */ + if ((buf == NULL) || (callback == NULL)) + return -1; + + decoder->fd = -1; + decoder->buf = (uint8_t *)buf; + decoder->dead = false; + if (bufsize == 0) { + decoder->bufsize = *(uint32_t *)buf; + debug("auto-detected %u byte object", decoder->bufsize); + } else { + decoder->bufsize = bufsize; + } + decoder->bufpos = 0; + decoder->callback = callback; + decoder->private = private; + decoder->nesting = 1; + decoder->pending = 0; + decoder->node.type = BSON_UNDEFINED; + + /* read and discard document size */ + if (read_int32(decoder, &len)) + CODER_KILL(decoder, "failed reading length"); + if ((len > 0) && (len > (int)decoder->bufsize)) + CODER_KILL(decoder, "document length larger than buffer"); + + /* ready for decoding */ + return 0; +} + +int +bson_decoder_next(bson_decoder_t decoder) +{ + int8_t tbyte; + int32_t tint; + unsigned nlen; + + CODER_CHECK(decoder); + + /* if the previous node was EOO, pop a nesting level */ + if (decoder->node.type == BSON_EOO) { + if (decoder->nesting > 0) + decoder->nesting--; + + /* if the nesting level is now zero, the top-level document is done */ + if (decoder->nesting == 0) { + /* like kill but not an error */ + debug("nesting is zero, document is done"); + decoder->fd = -1; + + /* return end-of-file to the caller */ + return 0; + } + } + + /* if there are unread bytes pending in the stream, discard them */ + while (decoder->pending > 0) { + if (read_int8(decoder, &tbyte)) + CODER_KILL(decoder, "read error discarding pending bytes"); + + decoder->pending--; + } + + /* get the type byte */ + if (read_int8(decoder, &tbyte)) + CODER_KILL(decoder, "read error on type byte"); + + decoder->node.type = tbyte; + decoder->pending = 0; + + debug("got type byte 0x%02x", decoder->node.type); + + /* EOO is special; it has no name/data following */ + if (decoder->node.type == BSON_EOO) { + decoder->node.name[0] = '\0'; + } else { + + /* get the node name */ + nlen = 0; + + for (;;) { + if (nlen >= BSON_MAXNAME) + CODER_KILL(decoder, "node name overflow"); + + if (read_int8(decoder, (int8_t *)&decoder->node.name[nlen])) + CODER_KILL(decoder, "read error on node name"); + + if (decoder->node.name[nlen] == '\0') + break; + + nlen++; + } + + debug("got name '%s'", decoder->node.name); + + switch (decoder->node.type) { + case BSON_BOOL: + if (read_int8(decoder, &tbyte)) + CODER_KILL(decoder, "read error on BSON_BOOL"); + decoder->node.b = (tbyte != 0); + break; + + case BSON_INT32: + if (read_int32(decoder, &tint)) + CODER_KILL(decoder, "read error on BSON_INT"); + decoder->node.i = tint; + break; + + case BSON_INT64: + if (read_int64(decoder, &decoder->node.i)) + CODER_KILL(decoder, "read error on BSON_INT"); + + break; + + case BSON_DOUBLE: + if (read_double(decoder, &decoder->node.d)) + CODER_KILL(decoder, "read error on BSON_DOUBLE"); + + break; + + case BSON_STRING: + if (read_int32(decoder, &decoder->pending)) + CODER_KILL(decoder, "read error on BSON_STRING length"); + break; + + case BSON_BINDATA: + if (read_int32(decoder, &decoder->pending)) + CODER_KILL(decoder, "read error on BSON_BINDATA size"); + + if (read_int8(decoder, &tbyte)) + CODER_KILL(decoder, "read error on BSON_BINDATA subtype"); + + decoder->node.subtype = tbyte; + break; + + /* XXX currently not supporting other types */ + default: + CODER_KILL(decoder, "unsupported node type"); + } + } + + /* call the callback and pass its results back */ + return decoder->callback(decoder, decoder->private, &decoder->node); +} + +int +bson_decoder_copy_data(bson_decoder_t decoder, void *buf) +{ + int result; + + CODER_CHECK(decoder); + + /* copy data */ + result = read_x(decoder, buf, decoder->pending); + + if (result != 0) + CODER_KILL(decoder, "read error on copy_data"); + + /* pending count is discharged */ + decoder->pending = 0; + return 0; +} + +size_t +bson_decoder_data_pending(bson_decoder_t decoder) +{ + return decoder->pending; +} + +static int +write_x(bson_encoder_t encoder, const void *p, size_t s) +{ + CODER_CHECK(encoder); + + if (encoder->fd > -1) + return (write(encoder->fd, p, s) == (int)s) ? 0 : -1; + + /* do we need to extend the buffer? */ + while ((encoder->bufpos + s) > encoder->bufsize) { + if (!encoder->realloc_ok) + CODER_KILL(encoder, "fixed-size buffer overflow"); + + uint8_t *newbuf = realloc(encoder->buf, encoder->bufsize + BSON_BUF_INCREMENT); + if (newbuf == NULL) + CODER_KILL(encoder, "could not grow buffer"); + + encoder->buf = newbuf; + encoder->bufsize += BSON_BUF_INCREMENT; + debug("allocated %d bytes", BSON_BUF_INCREMENT); + } + + memcpy(encoder->buf + encoder->bufpos, p, s); + encoder->bufpos += s; + debug("appended %d bytes", s); + + return 0; +} + +static int +write_int8(bson_encoder_t encoder, int8_t b) +{ + return write_x(encoder, &b, sizeof(b)); +} + +static int +write_int32(bson_encoder_t encoder, int32_t i) +{ + return write_x(encoder, &i, sizeof(i)); +} + +static int +write_int64(bson_encoder_t encoder, int64_t i) +{ + return write_x(encoder, &i, sizeof(i)); +} + +static int +write_double(bson_encoder_t encoder, double d) +{ + return write_x(encoder, &d, sizeof(d)); +} + +static int +write_name(bson_encoder_t encoder, const char *name) +{ + size_t len = strlen(name); + + if (len > BSON_MAXNAME) + CODER_KILL(encoder, "node name too long"); + + return write_x(encoder, name, len + 1); +} + +int +bson_encoder_init_file(bson_encoder_t encoder, int fd) +{ + encoder->fd = fd; + encoder->buf = NULL; + encoder->dead = false; + + if (write_int32(encoder, 0)) + CODER_KILL(encoder, "write error on document length"); + + return 0; +} + +int +bson_encoder_init_buf(bson_encoder_t encoder, void *buf, unsigned bufsize) +{ + encoder->fd = -1; + encoder->buf = (uint8_t *)buf; + encoder->bufpos = 0; + encoder->dead = false; + if (encoder->buf == NULL) { + encoder->bufsize = 0; + encoder->realloc_ok = true; + } else { + encoder->bufsize = bufsize; + encoder->realloc_ok = false; + } + + if (write_int32(encoder, 0)) + CODER_KILL(encoder, "write error on document length"); + + return 0; +} + +int +bson_encoder_fini(bson_encoder_t encoder) +{ + CODER_CHECK(encoder); + + if (write_int8(encoder, BSON_EOO)) + CODER_KILL(encoder, "write error on document terminator"); + + /* hack to fix up length for in-buffer documents */ + if (encoder->buf != NULL) { + int32_t len = bson_encoder_buf_size(encoder); + memcpy(encoder->buf, &len, sizeof(len)); + } + + return 0; +} + +int +bson_encoder_buf_size(bson_encoder_t encoder) +{ + CODER_CHECK(encoder); + + if (encoder->fd > -1) + return -1; + + return encoder->bufpos; +} + +void * +bson_encoder_buf_data(bson_encoder_t encoder) +{ + /* note, no CODER_CHECK here as the caller has to clean up dead buffers */ + + if (encoder->fd > -1) + return NULL; + + return encoder->buf; +} + +int bson_encoder_append_bool(bson_encoder_t encoder, const char *name, bool value) +{ + CODER_CHECK(encoder); + + if (write_int8(encoder, BSON_BOOL) || + write_name(encoder, name) || + write_int8(encoder, value ? 1 : 0)) + CODER_KILL(encoder, "write error on BSON_BOOL"); + + return 0; +} + +int +bson_encoder_append_int(bson_encoder_t encoder, const char *name, int64_t value) +{ + bool result; + + CODER_CHECK(encoder); + + /* use the smallest encoding that will hold the value */ + if (value == (int32_t)value) { + debug("encoding %lld as int32", value); + result = write_int8(encoder, BSON_INT32) || + write_name(encoder, name) || + write_int32(encoder, value); + } else { + debug("encoding %lld as int64", value); + result = write_int8(encoder, BSON_INT64) || + write_name(encoder, name) || + write_int64(encoder, value); + } + if (result) + CODER_KILL(encoder, "write error on BSON_INT"); + + return 0; +} + +int +bson_encoder_append_double(bson_encoder_t encoder, const char *name, double value) +{ + CODER_CHECK(encoder); + + if (write_int8(encoder, BSON_DOUBLE) || + write_name(encoder, name) || + write_double(encoder, value)) + CODER_KILL(encoder, "write error on BSON_DOUBLE"); + + + return 0; +} + +int +bson_encoder_append_string(bson_encoder_t encoder, const char *name, const char *string) +{ + size_t len; + + CODER_CHECK(encoder); + + len = strlen(string) + 1; /* include trailing nul */ + + if (write_int8(encoder, BSON_STRING) || + write_name(encoder, name) || + write_int32(encoder, len) || + write_x(encoder, string, len)) + CODER_KILL(encoder, "write error on BSON_STRING"); + + return 0; +} + +int +bson_encoder_append_binary(bson_encoder_t encoder, const char *name, bson_binary_subtype_t subtype, size_t size, const void *data) +{ + CODER_CHECK(encoder); + + if (write_int8(encoder, BSON_BINDATA) || + write_name(encoder, name) || + write_int32(encoder, size) || + write_int8(encoder, subtype) || + write_x(encoder, data, size)) + CODER_KILL(encoder, "write error on BSON_BINDATA"); + + return 0; +} -- cgit v1.2.3