dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity
px4-firmware/apps/systemlib/bson/tinybson.c

518 lines
12 KiB
C
Raw Blame History

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