Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-07 09:13:32 +08:00
Code Issues Actions 34 Packages Projects Releases Wiki Activity

Work in progress on to/from memory BSON coding.

Browse Source
This commit is contained in:
px4dev
2012-10-27 20:39:37 -07:00
parent 270a5d351f
commit c522b5446d
6 changed files with 423 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/systemlib/bson/tinybson.c
+158 -25
View File
@@ -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;
apps/systemlib/bson/tinybson.h
+76 -4
View File
@@ -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.
*/