diff --git a/cmp.c b/cmp.c new file mode 100644 index 0000000..89230b4 --- /dev/null +++ b/cmp.c @@ -0,0 +1,3352 @@ +/* +The MIT License (MIT) + +Copyright (c) 2017 Charles Gunyon + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +*/ + +#include +#include +#include + +#include "cmp.h" + +static const uint32_t version = 18; +static const uint32_t mp_version = 5; + +enum { + POSITIVE_FIXNUM_MARKER = 0x00, + FIXMAP_MARKER = 0x80, + FIXARRAY_MARKER = 0x90, + FIXSTR_MARKER = 0xA0, + NIL_MARKER = 0xC0, + FALSE_MARKER = 0xC2, + TRUE_MARKER = 0xC3, + BIN8_MARKER = 0xC4, + BIN16_MARKER = 0xC5, + BIN32_MARKER = 0xC6, + EXT8_MARKER = 0xC7, + EXT16_MARKER = 0xC8, + EXT32_MARKER = 0xC9, + FLOAT_MARKER = 0xCA, + DOUBLE_MARKER = 0xCB, + U8_MARKER = 0xCC, + U16_MARKER = 0xCD, + U32_MARKER = 0xCE, + U64_MARKER = 0xCF, + S8_MARKER = 0xD0, + S16_MARKER = 0xD1, + S32_MARKER = 0xD2, + S64_MARKER = 0xD3, + FIXEXT1_MARKER = 0xD4, + FIXEXT2_MARKER = 0xD5, + FIXEXT4_MARKER = 0xD6, + FIXEXT8_MARKER = 0xD7, + FIXEXT16_MARKER = 0xD8, + STR8_MARKER = 0xD9, + STR16_MARKER = 0xDA, + STR32_MARKER = 0xDB, + ARRAY16_MARKER = 0xDC, + ARRAY32_MARKER = 0xDD, + MAP16_MARKER = 0xDE, + MAP32_MARKER = 0xDF, + NEGATIVE_FIXNUM_MARKER = 0xE0 +}; + +enum { + FIXARRAY_SIZE = 0xF, + FIXMAP_SIZE = 0xF, + FIXSTR_SIZE = 0x1F +}; + +enum { + ERROR_NONE, + STR_DATA_LENGTH_TOO_LONG_ERROR, + BIN_DATA_LENGTH_TOO_LONG_ERROR, + ARRAY_LENGTH_TOO_LONG_ERROR, + MAP_LENGTH_TOO_LONG_ERROR, + INPUT_VALUE_TOO_LARGE_ERROR, + FIXED_VALUE_WRITING_ERROR, + TYPE_MARKER_READING_ERROR, + TYPE_MARKER_WRITING_ERROR, + DATA_READING_ERROR, + DATA_WRITING_ERROR, + EXT_TYPE_READING_ERROR, + EXT_TYPE_WRITING_ERROR, + INVALID_TYPE_ERROR, + LENGTH_READING_ERROR, + LENGTH_WRITING_ERROR, + SKIP_DEPTH_LIMIT_EXCEEDED_ERROR, + INTERNAL_ERROR, + ERROR_MAX +}; + +const char *cmp_error_messages[ERROR_MAX + 1] = { + "No Error", + "Specified string data length is too long (> 0xFFFFFFFF)", + "Specified binary data length is too long (> 0xFFFFFFFF)", + "Specified array length is too long (> 0xFFFFFFFF)", + "Specified map length is too long (> 0xFFFFFFFF)", + "Input value is too large", + "Error writing fixed value", + "Error reading type marker", + "Error writing type marker", + "Error reading packed data", + "Error writing packed data", + "Error reading ext type", + "Error writing ext type", + "Invalid type", + "Error reading size", + "Error writing size", + "Depth limit exceeded while skipping", + "Internal error", + "Max Error" +}; + +static const int32_t _i = 1; +#define is_bigendian() ((*(char *)&_i) == 0) + +static uint16_t be16(uint16_t x) { + char *b = (char *)&x; + + if (!is_bigendian()) { + char swap = 0; + + swap = b[0]; + b[0] = b[1]; + b[1] = swap; + } + + return x; +} + +static uint32_t be32(uint32_t x) { + char *b = (char *)&x; + + if (!is_bigendian()) { + char swap = 0; + + swap = b[0]; + b[0] = b[3]; + b[3] = swap; + + swap = b[1]; + b[1] = b[2]; + b[2] = swap; + } + + return x; +} + +static uint64_t be64(uint64_t x) { + char *b = (char *)&x; + + if (!is_bigendian()) { + char swap = 0; + + swap = b[0]; + b[0] = b[7]; + b[7] = swap; + + swap = b[1]; + b[1] = b[6]; + b[6] = swap; + + swap = b[2]; + b[2] = b[5]; + b[5] = swap; + + swap = b[3]; + b[3] = b[4]; + b[4] = swap; + } + + return x; +} + +static float decode_befloat(char *b) { + float f = 0.; + char *fb = (char *)&f; + + if (!is_bigendian()) { + fb[0] = b[3]; + fb[1] = b[2]; + fb[2] = b[1]; + fb[3] = b[0]; + } + + return f; +} + +static double decode_bedouble(char *b) { + double d = 0.; + char *db = (char *)&d; + + if (!is_bigendian()) { + db[0] = b[7]; + db[1] = b[6]; + db[2] = b[5]; + db[3] = b[4]; + db[4] = b[3]; + db[5] = b[2]; + db[6] = b[1]; + db[7] = b[0]; + } + + return d; +} + +static bool read_byte(cmp_ctx_t *ctx, uint8_t *x) { + return ctx->read(ctx, x, sizeof(uint8_t)); +} + +static bool write_byte(cmp_ctx_t *ctx, uint8_t x) { + return (ctx->write(ctx, &x, sizeof(uint8_t)) == (sizeof(uint8_t))); +} + +static bool skip_bytes(cmp_ctx_t *ctx, size_t count) { + if (ctx->skip) { + return ctx->skip(ctx, count); + } + else { + uint8_t floor; + size_t i; + + for (i = 0; i < count; i++) { + if (!ctx->read(ctx, &floor, sizeof(uint8_t))) { + return false; + } + } + + return true; + } +} + +static bool read_type_marker(cmp_ctx_t *ctx, uint8_t *marker) { + if (read_byte(ctx, marker)) { + return true; + } + + ctx->error = TYPE_MARKER_READING_ERROR; + return false; +} + +static bool write_type_marker(cmp_ctx_t *ctx, uint8_t marker) { + if (write_byte(ctx, marker)) + return true; + + ctx->error = TYPE_MARKER_WRITING_ERROR; + return false; +} + +static bool write_fixed_value(cmp_ctx_t *ctx, uint8_t value) { + if (write_byte(ctx, value)) + return true; + + ctx->error = FIXED_VALUE_WRITING_ERROR; + return false; +} + +static bool type_marker_to_cmp_type(uint8_t type_marker, uint8_t *cmp_type) { + if (type_marker <= 0x7F) { + *cmp_type = CMP_TYPE_POSITIVE_FIXNUM; + return true; + } + + if (type_marker <= 0x8F) { + *cmp_type = CMP_TYPE_FIXMAP; + return true; + } + + if (type_marker <= 0x9F) { + *cmp_type = CMP_TYPE_FIXARRAY; + return true; + } + + if (type_marker <= 0xBF) { + *cmp_type = CMP_TYPE_FIXSTR; + return true; + } + + if (type_marker >= 0xE0) { + *cmp_type = CMP_TYPE_NEGATIVE_FIXNUM; + return true; + } + + switch (type_marker) { + case NIL_MARKER: + *cmp_type = CMP_TYPE_NIL; + return true; + case FALSE_MARKER: + *cmp_type = CMP_TYPE_BOOLEAN; + return true; + case TRUE_MARKER: + *cmp_type = CMP_TYPE_BOOLEAN; + return true; + case BIN8_MARKER: + *cmp_type = CMP_TYPE_BIN8; + return true; + case BIN16_MARKER: + *cmp_type = CMP_TYPE_BIN16; + return true; + case BIN32_MARKER: + *cmp_type = CMP_TYPE_BIN32; + return true; + case EXT8_MARKER: + *cmp_type = CMP_TYPE_EXT8; + return true; + case EXT16_MARKER: + *cmp_type = CMP_TYPE_EXT16; + return true; + case EXT32_MARKER: + *cmp_type = CMP_TYPE_EXT32; + return true; + case FLOAT_MARKER: + *cmp_type = CMP_TYPE_FLOAT; + return true; + case DOUBLE_MARKER: + *cmp_type = CMP_TYPE_DOUBLE; + return true; + case U8_MARKER: + *cmp_type = CMP_TYPE_UINT8; + return true; + case U16_MARKER: + *cmp_type = CMP_TYPE_UINT16; + return true; + case U32_MARKER: + *cmp_type = CMP_TYPE_UINT32; + return true; + case U64_MARKER: + *cmp_type = CMP_TYPE_UINT64; + return true; + case S8_MARKER: + *cmp_type = CMP_TYPE_SINT8; + return true; + case S16_MARKER: + *cmp_type = CMP_TYPE_SINT16; + return true; + case S32_MARKER: + *cmp_type = CMP_TYPE_SINT32; + return true; + case S64_MARKER: + *cmp_type = CMP_TYPE_SINT64; + return true; + case FIXEXT1_MARKER: + *cmp_type = CMP_TYPE_FIXEXT1; + return true; + case FIXEXT2_MARKER: + *cmp_type = CMP_TYPE_FIXEXT2; + return true; + case FIXEXT4_MARKER: + *cmp_type = CMP_TYPE_FIXEXT4; + return true; + case FIXEXT8_MARKER: + *cmp_type = CMP_TYPE_FIXEXT8; + return true; + case FIXEXT16_MARKER: + *cmp_type = CMP_TYPE_FIXEXT16; + return true; + case STR8_MARKER: + *cmp_type = CMP_TYPE_STR8; + return true; + case STR16_MARKER: + *cmp_type = CMP_TYPE_STR16; + return true; + case STR32_MARKER: + *cmp_type = CMP_TYPE_STR32; + return true; + case ARRAY16_MARKER: + *cmp_type = CMP_TYPE_ARRAY16; + return true; + case ARRAY32_MARKER: + *cmp_type = CMP_TYPE_ARRAY32; + return true; + case MAP16_MARKER: + *cmp_type = CMP_TYPE_MAP16; + return true; + case MAP32_MARKER: + *cmp_type = CMP_TYPE_MAP32; + return true; + default: + return false; + } +} + +static bool read_type_size(cmp_ctx_t *ctx, uint8_t type_marker, + uint8_t cmp_type, + uint32_t *size) { + uint8_t u8temp = 0; + uint16_t u16temp = 0; + uint32_t u32temp = 0; + + switch (cmp_type) { + case CMP_TYPE_POSITIVE_FIXNUM: + *size = 0; + return true; + case CMP_TYPE_FIXMAP: + *size = (type_marker & FIXMAP_SIZE); + return true; + case CMP_TYPE_FIXARRAY: + *size = (type_marker & FIXARRAY_SIZE); + return true; + case CMP_TYPE_FIXSTR: + *size = (type_marker & FIXSTR_SIZE); + return true; + case CMP_TYPE_NIL: + *size = 0; + return true; + case CMP_TYPE_BOOLEAN: + *size = 0; + return true; + case CMP_TYPE_BIN8: + if (!ctx->read(ctx, &u8temp, sizeof(uint8_t))) { + ctx->error = LENGTH_READING_ERROR; + return false; + } + *size = u8temp; + return true; + case CMP_TYPE_BIN16: + if (!ctx->read(ctx, &u16temp, sizeof(uint16_t))) { + ctx->error = LENGTH_READING_ERROR; + return false; + } + *size = be16(u16temp); + return true; + case CMP_TYPE_BIN32: + if (!ctx->read(ctx, &u32temp, sizeof(uint32_t))) { + ctx->error = LENGTH_READING_ERROR; + return false; + } + *size = be32(u32temp); + return true; + case CMP_TYPE_EXT8: + if (!ctx->read(ctx, &u8temp, sizeof(uint8_t))) { + ctx->error = LENGTH_READING_ERROR; + return false; + } + *size = u8temp; + return true; + case CMP_TYPE_EXT16: + if (!ctx->read(ctx, &u16temp, sizeof(uint16_t))) { + ctx->error = LENGTH_READING_ERROR; + return false; + } + *size = be16(u16temp); + return true; + case CMP_TYPE_EXT32: + if (!ctx->read(ctx, &u32temp, sizeof(uint32_t))) { + ctx->error = LENGTH_READING_ERROR; + return false; + } + *size = be32(u32temp); + return true; + case CMP_TYPE_FLOAT: + *size = 4; + return true; + case CMP_TYPE_DOUBLE: + *size = 8; + return true; + case CMP_TYPE_UINT8: + *size = 1; + return true; + case CMP_TYPE_UINT16: + *size = 2; + return true; + case CMP_TYPE_UINT32: + *size = 4; + return true; + case CMP_TYPE_UINT64: + *size = 8; + return true; + case CMP_TYPE_SINT8: + *size = 1; + return true; + case CMP_TYPE_SINT16: + *size = 2; + return true; + case CMP_TYPE_SINT32: + *size = 4; + return true; + case CMP_TYPE_SINT64: + *size = 8; + return true; + case CMP_TYPE_FIXEXT1: + *size = 1; + return true; + case CMP_TYPE_FIXEXT2: + *size = 2; + return true; + case CMP_TYPE_FIXEXT4: + *size = 4; + return true; + case CMP_TYPE_FIXEXT8: + *size = 8; + return true; + case CMP_TYPE_FIXEXT16: + *size = 16; + return true; + case CMP_TYPE_STR8: + if (!ctx->read(ctx, &u8temp, sizeof(uint8_t))) { + ctx->error = DATA_READING_ERROR; + return false; + } + *size = u8temp; + return true; + case CMP_TYPE_STR16: + if (!ctx->read(ctx, &u16temp, sizeof(uint16_t))) { + ctx->error = DATA_READING_ERROR; + return false; + } + *size = be16(u16temp); + return true; + case CMP_TYPE_STR32: + if (!ctx->read(ctx, &u32temp, sizeof(uint32_t))) { + ctx->error = DATA_READING_ERROR; + return false; + } + *size = be32(u32temp); + return true; + case CMP_TYPE_ARRAY16: + if (!ctx->read(ctx, &u16temp, sizeof(uint16_t))) { + ctx->error = DATA_READING_ERROR; + return false; + } + *size = be16(u16temp); + return true; + case CMP_TYPE_ARRAY32: + if (!ctx->read(ctx, &u32temp, sizeof(uint32_t))) { + ctx->error = DATA_READING_ERROR; + return false; + } + *size = be32(u32temp); + return true; + case CMP_TYPE_MAP16: + if (!ctx->read(ctx, &u16temp, sizeof(uint16_t))) { + ctx->error = DATA_READING_ERROR; + return false; + } + *size = be16(u16temp); + return true; + case CMP_TYPE_MAP32: + if (!ctx->read(ctx, &u32temp, sizeof(uint32_t))) { + ctx->error = DATA_READING_ERROR; + return false; + } + *size = be32(u32temp); + return true; + case CMP_TYPE_NEGATIVE_FIXNUM: + *size = 0; + return true; + default: + ctx->error = INVALID_TYPE_ERROR; + return false; + } +} + +static bool read_obj_data(cmp_ctx_t *ctx, uint8_t type_marker, + cmp_object_t *obj) { + switch (obj->type) { + case CMP_TYPE_POSITIVE_FIXNUM: + obj->as.u8 = type_marker; + return true; + case CMP_TYPE_NEGATIVE_FIXNUM: + obj->as.s8 = type_marker; + return true; + case CMP_TYPE_NIL: + obj->as.u8 = 0; + return true; + case CMP_TYPE_BOOLEAN: + switch (type_marker) { + case TRUE_MARKER: + obj->as.boolean = true; + return true; + case FALSE_MARKER: + obj->as.boolean = false; + return true; + default: + break; + } + ctx->error = INTERNAL_ERROR; + return false; + case CMP_TYPE_UINT8: + if (!ctx->read(ctx, &obj->as.u8, sizeof(uint8_t))) { + ctx->error = DATA_READING_ERROR; + return false; + } + return true; + case CMP_TYPE_UINT16: + if (!ctx->read(ctx, &obj->as.u16, sizeof(uint16_t))) { + ctx->error = DATA_READING_ERROR; + return false; + } + obj->as.u16 = be16(obj->as.u16); + return true; + case CMP_TYPE_UINT32: + if (!ctx->read(ctx, &obj->as.u32, sizeof(uint32_t))) { + ctx->error = DATA_READING_ERROR; + return false; + } + obj->as.u32 = be32(obj->as.u32); + return true; + case CMP_TYPE_UINT64: + if (!ctx->read(ctx, &obj->as.u64, sizeof(uint64_t))) { + ctx->error = DATA_READING_ERROR; + return false; + } + obj->as.u64 = be64(obj->as.u64); + return true; + case CMP_TYPE_SINT8: + if (!ctx->read(ctx, &obj->as.s8, sizeof(int8_t))) { + ctx->error = DATA_READING_ERROR; + return false; + } + return true; + case CMP_TYPE_SINT16: + if (!ctx->read(ctx, &obj->as.s16, sizeof(int16_t))) { + ctx->error = DATA_READING_ERROR; + return false; + } + obj->as.s16 = be16(obj->as.s16); + return true; + case CMP_TYPE_SINT32: + if (!ctx->read(ctx, &obj->as.s32, sizeof(int32_t))) { + ctx->error = DATA_READING_ERROR; + return false; + } + obj->as.s32 = be32(obj->as.s32); + return true; + case CMP_TYPE_SINT64: + if (!ctx->read(ctx, &obj->as.s64, sizeof(int64_t))) { + ctx->error = DATA_READING_ERROR; + return false; + } + obj->as.s64 = be64(obj->as.s64); + return true; + case CMP_TYPE_FLOAT: + { + char bytes[4]; + + if (!ctx->read(ctx, bytes, 4)) { + ctx->error = DATA_READING_ERROR; + return false; + } + obj->as.flt = decode_befloat(bytes); + return true; + } + case CMP_TYPE_DOUBLE: + { + char bytes[8]; + + if (!ctx->read(ctx, bytes, 8)) { + ctx->error = DATA_READING_ERROR; + return false; + } + obj->as.dbl = decode_bedouble(bytes); + return true; + } + case CMP_TYPE_BIN8: + case CMP_TYPE_BIN16: + case CMP_TYPE_BIN32: + return read_type_size(ctx, type_marker, obj->type, &obj->as.bin_size); + case CMP_TYPE_FIXSTR: + case CMP_TYPE_STR8: + case CMP_TYPE_STR16: + case CMP_TYPE_STR32: + return read_type_size(ctx, type_marker, obj->type, &obj->as.str_size); + case CMP_TYPE_FIXARRAY: + case CMP_TYPE_ARRAY16: + case CMP_TYPE_ARRAY32: + return read_type_size(ctx, type_marker, obj->type, &obj->as.array_size); + case CMP_TYPE_FIXMAP: + case CMP_TYPE_MAP16: + case CMP_TYPE_MAP32: + return read_type_size(ctx, type_marker, obj->type, &obj->as.map_size); + case CMP_TYPE_FIXEXT1: + if (!ctx->read(ctx, &obj->as.ext.type, sizeof(int8_t))) { + ctx->error = EXT_TYPE_READING_ERROR; + return false; + } + obj->as.ext.size = 1; + return true; + case CMP_TYPE_FIXEXT2: + if (!ctx->read(ctx, &obj->as.ext.type, sizeof(int8_t))) { + ctx->error = EXT_TYPE_READING_ERROR; + return false; + } + obj->as.ext.size = 2; + return true; + case CMP_TYPE_FIXEXT4: + if (!ctx->read(ctx, &obj->as.ext.type, sizeof(int8_t))) { + ctx->error = EXT_TYPE_READING_ERROR; + return false; + } + obj->as.ext.size = 4; + return true; + case CMP_TYPE_FIXEXT8: + if (!ctx->read(ctx, &obj->as.ext.type, sizeof(int8_t))) { + ctx->error = EXT_TYPE_READING_ERROR; + return false; + } + obj->as.ext.size = 8; + return true; + case CMP_TYPE_FIXEXT16: + if (!ctx->read(ctx, &obj->as.ext.type, sizeof(int8_t))) { + ctx->error = EXT_TYPE_READING_ERROR; + return false; + } + obj->as.ext.size = 16; + return true; + case CMP_TYPE_EXT8: + if (!read_type_size(ctx, type_marker, obj->type, &obj->as.ext.size)) { + return false; + } + if (!ctx->read(ctx, &obj->as.ext.type, sizeof(int8_t))) { + ctx->error = EXT_TYPE_READING_ERROR; + return false; + } + return true; + case CMP_TYPE_EXT16: + if (!read_type_size(ctx, type_marker, obj->type, &obj->as.ext.size)) { + return false; + } + if (!ctx->read(ctx, &obj->as.ext.type, sizeof(int8_t))) { + ctx->error = EXT_TYPE_READING_ERROR; + return false; + } + obj->as.ext.type = obj->as.ext.type; + return true; + case CMP_TYPE_EXT32: + if (!read_type_size(ctx, type_marker, obj->type, &obj->as.ext.size)) { + return false; + } + if (!ctx->read(ctx, &obj->as.ext.type, sizeof(int8_t))) { + ctx->error = EXT_TYPE_READING_ERROR; + return false; + } + obj->as.ext.type = obj->as.ext.type; + return true; + default: + break; + } + + ctx->error = INVALID_TYPE_ERROR; + return false; +} + +void cmp_init(cmp_ctx_t *ctx, void *buf, cmp_reader read, + cmp_skipper skip, + cmp_writer write) { + ctx->error = ERROR_NONE; + ctx->buf = buf; + ctx->read = read; + ctx->skip = skip; + ctx->write = write; +} + +uint32_t cmp_version(void) { + return version; +} + +uint32_t cmp_mp_version(void) { + return mp_version; +} + +const char* cmp_strerror(cmp_ctx_t *ctx) { + if (ctx->error > ERROR_NONE && ctx->error < ERROR_MAX) + return cmp_error_messages[ctx->error]; + + return ""; +} + +bool cmp_write_pfix(cmp_ctx_t *ctx, uint8_t c) { + if (c <= 0x7F) + return write_fixed_value(ctx, c); + + ctx->error = INPUT_VALUE_TOO_LARGE_ERROR; + return false; +} + +bool cmp_write_nfix(cmp_ctx_t *ctx, int8_t c) { + if (c >= -32 && c <= -1) + return write_fixed_value(ctx, c); + + ctx->error = INPUT_VALUE_TOO_LARGE_ERROR; + return false; +} + +bool cmp_write_sfix(cmp_ctx_t *ctx, int8_t c) { + if (c >= 0) + return cmp_write_pfix(ctx, c); + if (c >= -32 && c <= -1) + return cmp_write_nfix(ctx, c); + + ctx->error = INPUT_VALUE_TOO_LARGE_ERROR; + return false; +} + +bool cmp_write_s8(cmp_ctx_t *ctx, int8_t c) { + if (!write_type_marker(ctx, S8_MARKER)) + return false; + + return ctx->write(ctx, &c, sizeof(int8_t)); +} + +bool cmp_write_s16(cmp_ctx_t *ctx, int16_t s) { + if (!write_type_marker(ctx, S16_MARKER)) + return false; + + s = be16(s); + + return ctx->write(ctx, &s, sizeof(int16_t)); +} + +bool cmp_write_s32(cmp_ctx_t *ctx, int32_t i) { + if (!write_type_marker(ctx, S32_MARKER)) + return false; + + i = be32(i); + + return ctx->write(ctx, &i, sizeof(int32_t)); +} + +bool cmp_write_s64(cmp_ctx_t *ctx, int64_t l) { + if (!write_type_marker(ctx, S64_MARKER)) + return false; + + l = be64(l); + + return ctx->write(ctx, &l, sizeof(int64_t)); +} + +bool cmp_write_integer(cmp_ctx_t *ctx, int64_t d) { + if (d >= 0) + return cmp_write_uinteger(ctx, d); + if (d >= -32) + return cmp_write_nfix(ctx, (int8_t)d); + if (d >= -128) + return cmp_write_s8(ctx, (int8_t)d); + if (d >= -32768) + return cmp_write_s16(ctx, (int16_t)d); + if (d >= (-2147483647 - 1)) + return cmp_write_s32(ctx, (int32_t)d); + + return cmp_write_s64(ctx, d); +} + +bool cmp_write_ufix(cmp_ctx_t *ctx, uint8_t c) { + return cmp_write_pfix(ctx, c); +} + +bool cmp_write_u8(cmp_ctx_t *ctx, uint8_t c) { + if (!write_type_marker(ctx, U8_MARKER)) + return false; + + return ctx->write(ctx, &c, sizeof(uint8_t)); +} + +bool cmp_write_u16(cmp_ctx_t *ctx, uint16_t s) { + if (!write_type_marker(ctx, U16_MARKER)) + return false; + + s = be16(s); + + return ctx->write(ctx, &s, sizeof(uint16_t)); +} + +bool cmp_write_u32(cmp_ctx_t *ctx, uint32_t i) { + if (!write_type_marker(ctx, U32_MARKER)) + return false; + + i = be32(i); + + return ctx->write(ctx, &i, sizeof(uint32_t)); +} + +bool cmp_write_u64(cmp_ctx_t *ctx, uint64_t l) { + if (!write_type_marker(ctx, U64_MARKER)) + return false; + + l = be64(l); + + return ctx->write(ctx, &l, sizeof(uint64_t)); +} + +bool cmp_write_uinteger(cmp_ctx_t *ctx, uint64_t u) { + if (u <= 0x7F) + return cmp_write_pfix(ctx, (uint8_t)u); + if (u <= 0xFF) + return cmp_write_u8(ctx, (uint8_t)u); + if (u <= 0xFFFF) + return cmp_write_u16(ctx, (uint16_t)u); + if (u <= 0xFFFFFFFF) + return cmp_write_u32(ctx, (uint32_t)u); + + return cmp_write_u64(ctx, u); +} + +bool cmp_write_float(cmp_ctx_t *ctx, float f) { + if (!write_type_marker(ctx, FLOAT_MARKER)) + return false; + + /* + * We may need to swap the float's bytes, but we can't just swap them inside + * the float because the swapped bytes may not constitute a valid float. + * Therefore, we have to create a buffer and swap the bytes there. + */ + if (!is_bigendian()) { + char swapped[sizeof(float)]; + char *fbuf = (char *)&f; + size_t i; + + for (i = 0; i < sizeof(float); i++) + swapped[i] = fbuf[sizeof(float) - i - 1]; + + return ctx->write(ctx, swapped, sizeof(float)); + } + + return ctx->write(ctx, &f, sizeof(float)); +} + +bool cmp_write_double(cmp_ctx_t *ctx, double d) { + if (!write_type_marker(ctx, DOUBLE_MARKER)) + return false; + + /* Same deal for doubles */ + if (!is_bigendian()) { + char swapped[sizeof(double)]; + char *dbuf = (char *)&d; + size_t i; + + for (i = 0; i < sizeof(double); i++) + swapped[i] = dbuf[sizeof(double) - i - 1]; + + return ctx->write(ctx, swapped, sizeof(double)); + } + + return ctx->write(ctx, &d, sizeof(double)); +} + +bool cmp_write_decimal(cmp_ctx_t *ctx, double d) { + float f = (float)d; + double df = (double)f; + + if (df == d) + return cmp_write_float(ctx, f); + else + return cmp_write_double(ctx, d); +} + +bool cmp_write_nil(cmp_ctx_t *ctx) { + return write_type_marker(ctx, NIL_MARKER); +} + +bool cmp_write_true(cmp_ctx_t *ctx) { + return write_type_marker(ctx, TRUE_MARKER); +} + +bool cmp_write_false(cmp_ctx_t *ctx) { + return write_type_marker(ctx, FALSE_MARKER); +} + +bool cmp_write_bool(cmp_ctx_t *ctx, bool b) { + if (b) + return cmp_write_true(ctx); + + return cmp_write_false(ctx); +} + +bool cmp_write_u8_as_bool(cmp_ctx_t *ctx, uint8_t b) { + if (b) + return cmp_write_true(ctx); + + return cmp_write_false(ctx); +} + +bool cmp_write_fixstr_marker(cmp_ctx_t *ctx, uint8_t size) { + if (size <= FIXSTR_SIZE) + return write_fixed_value(ctx, FIXSTR_MARKER | size); + + ctx->error = INPUT_VALUE_TOO_LARGE_ERROR; + return false; +} + +bool cmp_write_fixstr(cmp_ctx_t *ctx, const char *data, uint8_t size) { + if (!cmp_write_fixstr_marker(ctx, size)) + return false; + + if (size == 0) + return true; + + if (ctx->write(ctx, data, size)) + return true; + + ctx->error = DATA_WRITING_ERROR; + return false; +} + +bool cmp_write_str8_marker(cmp_ctx_t *ctx, uint8_t size) { + if (!write_type_marker(ctx, STR8_MARKER)) + return false; + + if (ctx->write(ctx, &size, sizeof(uint8_t))) + return true; + + ctx->error = LENGTH_WRITING_ERROR; + return false; +} + +bool cmp_write_str8(cmp_ctx_t *ctx, const char *data, uint8_t size) { + if (!cmp_write_str8_marker(ctx, size)) + return false; + + if (size == 0) + return true; + + if (ctx->write(ctx, data, size)) + return true; + + ctx->error = DATA_WRITING_ERROR; + return false; +} + +bool cmp_write_str16_marker(cmp_ctx_t *ctx, uint16_t size) { + if (!write_type_marker(ctx, STR16_MARKER)) + return false; + + size = be16(size); + + if (ctx->write(ctx, &size, sizeof(uint16_t))) + return true; + + ctx->error = LENGTH_WRITING_ERROR; + return false; +} + +bool cmp_write_str16(cmp_ctx_t *ctx, const char *data, uint16_t size) { + if (!cmp_write_str16_marker(ctx, size)) + return false; + + if (size == 0) + return true; + + if (ctx->write(ctx, data, size)) + return true; + + ctx->error = DATA_WRITING_ERROR; + return false; +} + +bool cmp_write_str32_marker(cmp_ctx_t *ctx, uint32_t size) { + if (!write_type_marker(ctx, STR32_MARKER)) + return false; + + size = be32(size); + + if (ctx->write(ctx, &size, sizeof(uint32_t))) + return true; + + ctx->error = LENGTH_WRITING_ERROR; + return false; +} + +bool cmp_write_str32(cmp_ctx_t *ctx, const char *data, uint32_t size) { + if (!cmp_write_str32_marker(ctx, size)) + return false; + + if (size == 0) + return true; + + if (ctx->write(ctx, data, size)) + return true; + + ctx->error = DATA_WRITING_ERROR; + return false; +} + +bool cmp_write_str_marker(cmp_ctx_t *ctx, uint32_t size) { + if (size <= FIXSTR_SIZE) + return cmp_write_fixstr_marker(ctx, (uint8_t)size); + if (size <= 0xFF) + return cmp_write_str8_marker(ctx, (uint8_t)size); + if (size <= 0xFFFF) + return cmp_write_str16_marker(ctx, (uint16_t)size); + + return cmp_write_str32_marker(ctx, size); +} + +bool cmp_write_str_marker_v4(cmp_ctx_t *ctx, uint32_t size) { + if (size <= FIXSTR_SIZE) + return cmp_write_fixstr_marker(ctx, (uint8_t)size); + if (size <= 0xFFFF) + return cmp_write_str16_marker(ctx, (uint16_t)size); + + return cmp_write_str32_marker(ctx, size); +} + +bool cmp_write_str(cmp_ctx_t *ctx, const char *data, uint32_t size) { + if (size <= FIXSTR_SIZE) + return cmp_write_fixstr(ctx, data, (uint8_t)size); + if (size <= 0xFF) + return cmp_write_str8(ctx, data, (uint8_t)size); + if (size <= 0xFFFF) + return cmp_write_str16(ctx, data, (uint16_t)size); + + return cmp_write_str32(ctx, data, size); +} + +bool cmp_write_str_v4(cmp_ctx_t *ctx, const char *data, uint32_t size) { + if (size <= FIXSTR_SIZE) + return cmp_write_fixstr(ctx, data, (uint8_t)size); + if (size <= 0xFFFF) + return cmp_write_str16(ctx, data, (uint16_t)size); + + return cmp_write_str32(ctx, data, size); +} + +bool cmp_write_bin8_marker(cmp_ctx_t *ctx, uint8_t size) { + if (!write_type_marker(ctx, BIN8_MARKER)) + return false; + + if (ctx->write(ctx, &size, sizeof(uint8_t))) + return true; + + ctx->error = LENGTH_WRITING_ERROR; + return false; +} + +bool cmp_write_bin8(cmp_ctx_t *ctx, const void *data, uint8_t size) { + if (!cmp_write_bin8_marker(ctx, size)) + return false; + + if (size == 0) + return true; + + if (ctx->write(ctx, data, size)) + return true; + + ctx->error = DATA_WRITING_ERROR; + return false; +} + +bool cmp_write_bin16_marker(cmp_ctx_t *ctx, uint16_t size) { + if (!write_type_marker(ctx, BIN16_MARKER)) + return false; + + size = be16(size); + + if (ctx->write(ctx, &size, sizeof(uint16_t))) + return true; + + ctx->error = LENGTH_WRITING_ERROR; + return false; +} + +bool cmp_write_bin16(cmp_ctx_t *ctx, const void *data, uint16_t size) { + if (!cmp_write_bin16_marker(ctx, size)) + return false; + + if (size == 0) + return true; + + if (ctx->write(ctx, data, size)) + return true; + + ctx->error = DATA_WRITING_ERROR; + return false; +} + +bool cmp_write_bin32_marker(cmp_ctx_t *ctx, uint32_t size) { + if (!write_type_marker(ctx, BIN32_MARKER)) + return false; + + size = be32(size); + + if (ctx->write(ctx, &size, sizeof(uint32_t))) + return true; + + ctx->error = LENGTH_WRITING_ERROR; + return false; +} + +bool cmp_write_bin32(cmp_ctx_t *ctx, const void *data, uint32_t size) { + if (!cmp_write_bin32_marker(ctx, size)) + return false; + + if (size == 0) + return true; + + if (ctx->write(ctx, data, size)) + return true; + + ctx->error = DATA_WRITING_ERROR; + return false; +} + +bool cmp_write_bin_marker(cmp_ctx_t *ctx, uint32_t size) { + if (size <= 0xFF) + return cmp_write_bin8_marker(ctx, (uint8_t)size); + if (size <= 0xFFFF) + return cmp_write_bin16_marker(ctx, (uint16_t)size); + + return cmp_write_bin32_marker(ctx, size); +} + +bool cmp_write_bin(cmp_ctx_t *ctx, const void *data, uint32_t size) { + if (size <= 0xFF) + return cmp_write_bin8(ctx, data, (uint8_t)size); + if (size <= 0xFFFF) + return cmp_write_bin16(ctx, data, (uint16_t)size); + + return cmp_write_bin32(ctx, data, size); +} + +bool cmp_write_fixarray(cmp_ctx_t *ctx, uint8_t size) { + if (size <= FIXARRAY_SIZE) + return write_fixed_value(ctx, FIXARRAY_MARKER | size); + + ctx->error = INPUT_VALUE_TOO_LARGE_ERROR; + return false; +} + +bool cmp_write_array16(cmp_ctx_t *ctx, uint16_t size) { + if (!write_type_marker(ctx, ARRAY16_MARKER)) + return false; + + size = be16(size); + + if (ctx->write(ctx, &size, sizeof(uint16_t))) + return true; + + ctx->error = LENGTH_WRITING_ERROR; + return false; +} + +bool cmp_write_array32(cmp_ctx_t *ctx, uint32_t size) { + if (!write_type_marker(ctx, ARRAY32_MARKER)) + return false; + + size = be32(size); + + if (ctx->write(ctx, &size, sizeof(uint32_t))) + return true; + + ctx->error = LENGTH_WRITING_ERROR; + return false; +} + +bool cmp_write_array(cmp_ctx_t *ctx, uint32_t size) { + if (size <= FIXARRAY_SIZE) + return cmp_write_fixarray(ctx, (uint8_t)size); + if (size <= 0xFFFF) + return cmp_write_array16(ctx, (uint16_t)size); + + return cmp_write_array32(ctx, size); +} + +bool cmp_write_fixmap(cmp_ctx_t *ctx, uint8_t size) { + if (size <= FIXMAP_SIZE) + return write_fixed_value(ctx, FIXMAP_MARKER | size); + + ctx->error = INPUT_VALUE_TOO_LARGE_ERROR; + return false; +} + +bool cmp_write_map16(cmp_ctx_t *ctx, uint16_t size) { + if (!write_type_marker(ctx, MAP16_MARKER)) + return false; + + size = be16(size); + + if (ctx->write(ctx, &size, sizeof(uint16_t))) + return true; + + ctx->error = LENGTH_WRITING_ERROR; + return false; +} + +bool cmp_write_map32(cmp_ctx_t *ctx, uint32_t size) { + if (!write_type_marker(ctx, MAP32_MARKER)) + return false; + + size = be32(size); + + if (ctx->write(ctx, &size, sizeof(uint32_t))) + return true; + + ctx->error = LENGTH_WRITING_ERROR; + return false; +} + +bool cmp_write_map(cmp_ctx_t *ctx, uint32_t size) { + if (size <= FIXMAP_SIZE) + return cmp_write_fixmap(ctx, (uint8_t)size); + if (size <= 0xFFFF) + return cmp_write_map16(ctx, (uint16_t)size); + + return cmp_write_map32(ctx, size); +} + +bool cmp_write_fixext1_marker(cmp_ctx_t *ctx, int8_t type) { + if (!write_type_marker(ctx, FIXEXT1_MARKER)) + return false; + + if (ctx->write(ctx, &type, sizeof(int8_t))) + return true; + + ctx->error = EXT_TYPE_WRITING_ERROR; + return false; +} + +bool cmp_write_fixext1(cmp_ctx_t *ctx, int8_t type, const void *data) { + if (!cmp_write_fixext1_marker(ctx, type)) + return false; + + if (ctx->write(ctx, data, 1)) + return true; + + ctx->error = DATA_WRITING_ERROR; + return false; +} + +bool cmp_write_fixext2_marker(cmp_ctx_t *ctx, int8_t type) { + if (!write_type_marker(ctx, FIXEXT2_MARKER)) + return false; + + if (ctx->write(ctx, &type, sizeof(int8_t))) + return true; + + ctx->error = EXT_TYPE_WRITING_ERROR; + return false; +} + +bool cmp_write_fixext2(cmp_ctx_t *ctx, int8_t type, const void *data) { + if (!cmp_write_fixext2_marker(ctx, type)) + return false; + + if (ctx->write(ctx, data, 2)) + return true; + + ctx->error = DATA_WRITING_ERROR; + return false; +} + +bool cmp_write_fixext4_marker(cmp_ctx_t *ctx, int8_t type) { + if (!write_type_marker(ctx, FIXEXT4_MARKER)) + return false; + + if (ctx->write(ctx, &type, sizeof(int8_t))) + return true; + + ctx->error = EXT_TYPE_WRITING_ERROR; + return false; +} + +bool cmp_write_fixext4(cmp_ctx_t *ctx, int8_t type, const void *data) { + if (!cmp_write_fixext4_marker(ctx, type)) + return false; + + if (ctx->write(ctx, data, 4)) + return true; + + ctx->error = DATA_WRITING_ERROR; + return false; +} + +bool cmp_write_fixext8_marker(cmp_ctx_t *ctx, int8_t type) { + if (!write_type_marker(ctx, FIXEXT8_MARKER)) + return false; + + if (ctx->write(ctx, &type, sizeof(int8_t))) + return true; + + ctx->error = EXT_TYPE_WRITING_ERROR; + return false; +} + +bool cmp_write_fixext8(cmp_ctx_t *ctx, int8_t type, const void *data) { + if (!cmp_write_fixext8_marker(ctx, type)) + return false; + + if (ctx->write(ctx, data, 8)) + return true; + + ctx->error = DATA_WRITING_ERROR; + return false; +} + +bool cmp_write_fixext16_marker(cmp_ctx_t *ctx, int8_t type) { + if (!write_type_marker(ctx, FIXEXT16_MARKER)) + return false; + + if (ctx->write(ctx, &type, sizeof(int8_t))) + return true; + + ctx->error = EXT_TYPE_WRITING_ERROR; + return false; +} + +bool cmp_write_fixext16(cmp_ctx_t *ctx, int8_t type, const void *data) { + if (!cmp_write_fixext16_marker(ctx, type)) + return false; + + if (ctx->write(ctx, data, 16)) + return true; + + ctx->error = DATA_WRITING_ERROR; + return false; +} + +bool cmp_write_ext8_marker(cmp_ctx_t *ctx, int8_t type, uint8_t size) { + if (!write_type_marker(ctx, EXT8_MARKER)) + return false; + + if (!ctx->write(ctx, &size, sizeof(uint8_t))) { + ctx->error = LENGTH_WRITING_ERROR; + return false; + } + + if (ctx->write(ctx, &type, sizeof(int8_t))) + return true; + + ctx->error = EXT_TYPE_WRITING_ERROR; + return false; +} + +bool cmp_write_ext8(cmp_ctx_t *ctx, int8_t tp, uint8_t sz, const void *data) { + if (!cmp_write_ext8_marker(ctx, tp, sz)) + return false; + + if (ctx->write(ctx, data, sz)) + return true; + + ctx->error = DATA_WRITING_ERROR; + return false; +} + +bool cmp_write_ext16_marker(cmp_ctx_t *ctx, int8_t type, uint16_t size) { + if (!write_type_marker(ctx, EXT16_MARKER)) + return false; + + size = be16(size); + + if (!ctx->write(ctx, &size, sizeof(uint16_t))) { + ctx->error = LENGTH_WRITING_ERROR; + return false; + } + + if (ctx->write(ctx, &type, sizeof(int8_t))) + return true; + + ctx->error = EXT_TYPE_WRITING_ERROR; + return false; +} + +bool cmp_write_ext16(cmp_ctx_t *ctx, int8_t tp, uint16_t sz, const void *data) { + if (!cmp_write_ext16_marker(ctx, tp, sz)) + return false; + + if (ctx->write(ctx, data, sz)) + return true; + + ctx->error = DATA_WRITING_ERROR; + return false; +} + +bool cmp_write_ext32_marker(cmp_ctx_t *ctx, int8_t type, uint32_t size) { + if (!write_type_marker(ctx, EXT32_MARKER)) + return false; + + size = be32(size); + + if (!ctx->write(ctx, &size, sizeof(uint32_t))) { + ctx->error = LENGTH_WRITING_ERROR; + return false; + } + + if (ctx->write(ctx, &type, sizeof(int8_t))) + return true; + + ctx->error = EXT_TYPE_WRITING_ERROR; + return false; +} + +bool cmp_write_ext32(cmp_ctx_t *ctx, int8_t tp, uint32_t sz, const void *data) { + if (!cmp_write_ext32_marker(ctx, tp, sz)) + return false; + + if (ctx->write(ctx, data, sz)) + return true; + + ctx->error = DATA_WRITING_ERROR; + return false; +} + +bool cmp_write_ext_marker(cmp_ctx_t *ctx, int8_t tp, uint32_t sz) { + if (sz == 1) + return cmp_write_fixext1_marker(ctx, tp); + if (sz == 2) + return cmp_write_fixext2_marker(ctx, tp); + if (sz == 4) + return cmp_write_fixext4_marker(ctx, tp); + if (sz == 8) + return cmp_write_fixext8_marker(ctx, tp); + if (sz == 16) + return cmp_write_fixext16_marker(ctx, tp); + if (sz <= 0xFF) + return cmp_write_ext8_marker(ctx, tp, (uint8_t)sz); + if (sz <= 0xFFFF) + return cmp_write_ext16_marker(ctx, tp, (uint16_t)sz); + + return cmp_write_ext32_marker(ctx, tp, sz); +} + +bool cmp_write_ext(cmp_ctx_t *ctx, int8_t tp, uint32_t sz, const void *data) { + if (sz == 1) + return cmp_write_fixext1(ctx, tp, data); + if (sz == 2) + return cmp_write_fixext2(ctx, tp, data); + if (sz == 4) + return cmp_write_fixext4(ctx, tp, data); + if (sz == 8) + return cmp_write_fixext8(ctx, tp, data); + if (sz == 16) + return cmp_write_fixext16(ctx, tp, data); + if (sz <= 0xFF) + return cmp_write_ext8(ctx, tp, (uint8_t)sz, data); + if (sz <= 0xFFFF) + return cmp_write_ext16(ctx, tp, (uint16_t)sz, data); + + return cmp_write_ext32(ctx, tp, sz, data); +} + +bool cmp_write_object(cmp_ctx_t *ctx, cmp_object_t *obj) { + switch(obj->type) { + case CMP_TYPE_POSITIVE_FIXNUM: + return cmp_write_pfix(ctx, obj->as.u8); + case CMP_TYPE_FIXMAP: + return cmp_write_fixmap(ctx, (uint8_t)obj->as.map_size); + case CMP_TYPE_FIXARRAY: + return cmp_write_fixarray(ctx, (uint8_t)obj->as.array_size); + case CMP_TYPE_FIXSTR: + return cmp_write_fixstr_marker(ctx, (uint8_t)obj->as.str_size); + case CMP_TYPE_NIL: + return cmp_write_nil(ctx); + case CMP_TYPE_BOOLEAN: + if (obj->as.boolean) + return cmp_write_true(ctx); + return cmp_write_false(ctx); + case CMP_TYPE_BIN8: + return cmp_write_bin8_marker(ctx, (uint8_t)obj->as.bin_size); + case CMP_TYPE_BIN16: + return cmp_write_bin16_marker(ctx, (uint16_t)obj->as.bin_size); + case CMP_TYPE_BIN32: + return cmp_write_bin32_marker(ctx, obj->as.bin_size); + case CMP_TYPE_EXT8: + return cmp_write_ext8_marker( + ctx, obj->as.ext.type, (uint8_t)obj->as.ext.size + ); + case CMP_TYPE_EXT16: + return cmp_write_ext16_marker( + ctx, obj->as.ext.type, (uint16_t)obj->as.ext.size + ); + case CMP_TYPE_EXT32: + return cmp_write_ext32_marker(ctx, obj->as.ext.type, obj->as.ext.size); + case CMP_TYPE_FLOAT: + return cmp_write_float(ctx, obj->as.flt); + case CMP_TYPE_DOUBLE: + return cmp_write_double(ctx, obj->as.dbl); + case CMP_TYPE_UINT8: + return cmp_write_u8(ctx, obj->as.u8); + case CMP_TYPE_UINT16: + return cmp_write_u16(ctx, obj->as.u16); + case CMP_TYPE_UINT32: + return cmp_write_u32(ctx, obj->as.u32); + case CMP_TYPE_UINT64: + return cmp_write_u64(ctx, obj->as.u64); + case CMP_TYPE_SINT8: + return cmp_write_s8(ctx, obj->as.s8); + case CMP_TYPE_SINT16: + return cmp_write_s16(ctx, obj->as.s16); + case CMP_TYPE_SINT32: + return cmp_write_s32(ctx, obj->as.s32); + case CMP_TYPE_SINT64: + return cmp_write_s64(ctx, obj->as.s64); + case CMP_TYPE_FIXEXT1: + return cmp_write_fixext1_marker(ctx, obj->as.ext.type); + case CMP_TYPE_FIXEXT2: + return cmp_write_fixext2_marker(ctx, obj->as.ext.type); + case CMP_TYPE_FIXEXT4: + return cmp_write_fixext4_marker(ctx, obj->as.ext.type); + case CMP_TYPE_FIXEXT8: + return cmp_write_fixext8_marker(ctx, obj->as.ext.type); + case CMP_TYPE_FIXEXT16: + return cmp_write_fixext16_marker(ctx, obj->as.ext.type); + case CMP_TYPE_STR8: + return cmp_write_str8_marker(ctx, (uint8_t)obj->as.str_size); + case CMP_TYPE_STR16: + return cmp_write_str16_marker(ctx, (uint16_t)obj->as.str_size); + case CMP_TYPE_STR32: + return cmp_write_str32_marker(ctx, obj->as.str_size); + case CMP_TYPE_ARRAY16: + return cmp_write_array16(ctx, (uint16_t)obj->as.array_size); + case CMP_TYPE_ARRAY32: + return cmp_write_array32(ctx, obj->as.array_size); + case CMP_TYPE_MAP16: + return cmp_write_map16(ctx, (uint16_t)obj->as.map_size); + case CMP_TYPE_MAP32: + return cmp_write_map32(ctx, obj->as.map_size); + case CMP_TYPE_NEGATIVE_FIXNUM: + return cmp_write_nfix(ctx, obj->as.s8); + default: + ctx->error = INVALID_TYPE_ERROR; + return false; + } +} + +bool cmp_write_object_v4(cmp_ctx_t *ctx, cmp_object_t *obj) { + switch(obj->type) { + case CMP_TYPE_POSITIVE_FIXNUM: + return cmp_write_pfix(ctx, obj->as.u8); + case CMP_TYPE_FIXMAP: + return cmp_write_fixmap(ctx, (uint8_t)obj->as.map_size); + case CMP_TYPE_FIXARRAY: + return cmp_write_fixarray(ctx, (uint8_t)obj->as.array_size); + case CMP_TYPE_FIXSTR: + return cmp_write_fixstr_marker(ctx, (uint8_t)obj->as.str_size); + case CMP_TYPE_NIL: + return cmp_write_nil(ctx); + case CMP_TYPE_BOOLEAN: + if (obj->as.boolean) + return cmp_write_true(ctx); + return cmp_write_false(ctx); + case CMP_TYPE_EXT8: + return cmp_write_ext8_marker(ctx, obj->as.ext.type, (uint8_t)obj->as.ext.size); + case CMP_TYPE_EXT16: + return cmp_write_ext16_marker( + ctx, obj->as.ext.type, (uint16_t)obj->as.ext.size + ); + case CMP_TYPE_EXT32: + return cmp_write_ext32_marker(ctx, obj->as.ext.type, obj->as.ext.size); + case CMP_TYPE_FLOAT: + return cmp_write_float(ctx, obj->as.flt); + case CMP_TYPE_DOUBLE: + return cmp_write_double(ctx, obj->as.dbl); + case CMP_TYPE_UINT8: + return cmp_write_u8(ctx, obj->as.u8); + case CMP_TYPE_UINT16: + return cmp_write_u16(ctx, obj->as.u16); + case CMP_TYPE_UINT32: + return cmp_write_u32(ctx, obj->as.u32); + case CMP_TYPE_UINT64: + return cmp_write_u64(ctx, obj->as.u64); + case CMP_TYPE_SINT8: + return cmp_write_s8(ctx, obj->as.s8); + case CMP_TYPE_SINT16: + return cmp_write_s16(ctx, obj->as.s16); + case CMP_TYPE_SINT32: + return cmp_write_s32(ctx, obj->as.s32); + case CMP_TYPE_SINT64: + return cmp_write_s64(ctx, obj->as.s64); + case CMP_TYPE_FIXEXT1: + return cmp_write_fixext1_marker(ctx, obj->as.ext.type); + case CMP_TYPE_FIXEXT2: + return cmp_write_fixext2_marker(ctx, obj->as.ext.type); + case CMP_TYPE_FIXEXT4: + return cmp_write_fixext4_marker(ctx, obj->as.ext.type); + case CMP_TYPE_FIXEXT8: + return cmp_write_fixext8_marker(ctx, obj->as.ext.type); + case CMP_TYPE_FIXEXT16: + return cmp_write_fixext16_marker(ctx, obj->as.ext.type); + case CMP_TYPE_STR16: + return cmp_write_str16_marker(ctx, (uint16_t)obj->as.str_size); + case CMP_TYPE_STR32: + return cmp_write_str32_marker(ctx, obj->as.str_size); + case CMP_TYPE_ARRAY16: + return cmp_write_array16(ctx, (uint16_t)obj->as.array_size); + case CMP_TYPE_ARRAY32: + return cmp_write_array32(ctx, obj->as.array_size); + case CMP_TYPE_MAP16: + return cmp_write_map16(ctx, (uint16_t)obj->as.map_size); + case CMP_TYPE_MAP32: + return cmp_write_map32(ctx, obj->as.map_size); + case CMP_TYPE_NEGATIVE_FIXNUM: + return cmp_write_nfix(ctx, obj->as.s8); + default: + ctx->error = INVALID_TYPE_ERROR; + return false; + } +} + +bool cmp_read_pfix(cmp_ctx_t *ctx, uint8_t *c) { + cmp_object_t obj; + + if (!cmp_read_object(ctx, &obj)) + return false; + + if (obj.type != CMP_TYPE_POSITIVE_FIXNUM) { + ctx->error = INVALID_TYPE_ERROR; + return false; + } + + *c = obj.as.u8; + return true; +} + +bool cmp_read_nfix(cmp_ctx_t *ctx, int8_t *c) { + cmp_object_t obj; + + if (!cmp_read_object(ctx, &obj)) + return false; + + if (obj.type != CMP_TYPE_NEGATIVE_FIXNUM) { + ctx->error = INVALID_TYPE_ERROR; + return false; + } + + *c = obj.as.s8; + return true; +} + +bool cmp_read_sfix(cmp_ctx_t *ctx, int8_t *c) { + cmp_object_t obj; + + if (!cmp_read_object(ctx, &obj)) + return false; + + switch (obj.type) { + case CMP_TYPE_POSITIVE_FIXNUM: + case CMP_TYPE_NEGATIVE_FIXNUM: + *c = obj.as.s8; + return true; + default: + ctx->error = INVALID_TYPE_ERROR; + return false; + } +} + +bool cmp_read_s8(cmp_ctx_t *ctx, int8_t *c) { + cmp_object_t obj; + + if (!cmp_read_object(ctx, &obj)) + return false; + + if (obj.type != CMP_TYPE_SINT8) { + ctx->error = INVALID_TYPE_ERROR; + return false; + } + + *c = obj.as.s8; + return true; +} + +bool cmp_read_s16(cmp_ctx_t *ctx, int16_t *s) { + cmp_object_t obj; + + if (!cmp_read_object(ctx, &obj)) + return false; + + if (obj.type != CMP_TYPE_SINT16) { + ctx->error = INVALID_TYPE_ERROR; + return false; + } + + *s = obj.as.s16; + return true; +} + +bool cmp_read_s32(cmp_ctx_t *ctx, int32_t *i) { + cmp_object_t obj; + + if (!cmp_read_object(ctx, &obj)) + return false; + + if (obj.type != CMP_TYPE_SINT32) { + ctx->error = INVALID_TYPE_ERROR; + return false; + } + + *i = obj.as.s32; + return true; +} + +bool cmp_read_s64(cmp_ctx_t *ctx, int64_t *l) { + cmp_object_t obj; + + if (!cmp_read_object(ctx, &obj)) + return false; + + if (obj.type != CMP_TYPE_SINT64) { + ctx->error = INVALID_TYPE_ERROR; + return false; + } + + *l = obj.as.s64; + return true; +} + +bool cmp_read_char(cmp_ctx_t *ctx, int8_t *c) { + cmp_object_t obj; + + if (!cmp_read_object(ctx, &obj)) + return false; + + switch (obj.type) { + case CMP_TYPE_POSITIVE_FIXNUM: + case CMP_TYPE_NEGATIVE_FIXNUM: + case CMP_TYPE_SINT8: + *c = obj.as.s8; + return true; + case CMP_TYPE_UINT8: + if (obj.as.u8 <= 127) { + *c = obj.as.u8; + return true; + } + break; + default: + break; + } + + ctx->error = INVALID_TYPE_ERROR; + return false; +} + +bool cmp_read_short(cmp_ctx_t *ctx, int16_t *s) { + cmp_object_t obj; + + if (!cmp_read_object(ctx, &obj)) + return false; + + switch (obj.type) { + case CMP_TYPE_POSITIVE_FIXNUM: + case CMP_TYPE_NEGATIVE_FIXNUM: + case CMP_TYPE_SINT8: + *s = obj.as.s8; + return true; + case CMP_TYPE_UINT8: + *s = obj.as.u8; + return true; + case CMP_TYPE_SINT16: + *s = obj.as.s16; + return true; + case CMP_TYPE_UINT16: + if (obj.as.u16 <= 32767) { + *s = obj.as.u16; + return true; + } + break; + default: + break; + } + + ctx->error = INVALID_TYPE_ERROR; + return false; +} + +bool cmp_read_int(cmp_ctx_t *ctx, int32_t *i) { + cmp_object_t obj; + + if (!cmp_read_object(ctx, &obj)) + return false; + + switch (obj.type) { + case CMP_TYPE_POSITIVE_FIXNUM: + case CMP_TYPE_NEGATIVE_FIXNUM: + case CMP_TYPE_SINT8: + *i = obj.as.s8; + return true; + case CMP_TYPE_UINT8: + *i = obj.as.u8; + return true; + case CMP_TYPE_SINT16: + *i = obj.as.s16; + return true; + case CMP_TYPE_UINT16: + *i = obj.as.u16; + return true; + case CMP_TYPE_SINT32: + *i = obj.as.s32; + return true; + case CMP_TYPE_UINT32: + if (obj.as.u32 <= 2147483647) { + *i = obj.as.u32; + return true; + } + break; + default: + break; + } + + ctx->error = INVALID_TYPE_ERROR; + return false; +} + +bool cmp_read_long(cmp_ctx_t *ctx, int64_t *d) { + cmp_object_t obj; + + if (!cmp_read_object(ctx, &obj)) + return false; + + switch (obj.type) { + case CMP_TYPE_POSITIVE_FIXNUM: + case CMP_TYPE_NEGATIVE_FIXNUM: + case CMP_TYPE_SINT8: + *d = obj.as.s8; + return true; + case CMP_TYPE_UINT8: + *d = obj.as.u8; + return true; + case CMP_TYPE_SINT16: + *d = obj.as.s16; + return true; + case CMP_TYPE_UINT16: + *d = obj.as.u16; + return true; + case CMP_TYPE_SINT32: + *d = obj.as.s32; + return true; + case CMP_TYPE_UINT32: + *d = obj.as.u32; + return true; + case CMP_TYPE_SINT64: + *d = obj.as.s64; + return true; + case CMP_TYPE_UINT64: + if (obj.as.u64 <= 9223372036854775807) { + *d = obj.as.u64; + return true; + } + break; + default: + break; + } + + ctx->error = INVALID_TYPE_ERROR; + return false; +} + +bool cmp_read_integer(cmp_ctx_t *ctx, int64_t *d) { + return cmp_read_long(ctx, d); +} + +bool cmp_read_ufix(cmp_ctx_t *ctx, uint8_t *c) { + return cmp_read_pfix(ctx, c); +} + +bool cmp_read_u8(cmp_ctx_t *ctx, uint8_t *c) { + cmp_object_t obj; + + if (!cmp_read_object(ctx, &obj)) + return false; + + if (obj.type != CMP_TYPE_UINT8) { + ctx->error = INVALID_TYPE_ERROR; + return false; + } + + *c = obj.as.u8; + return true; +} + +bool cmp_read_u16(cmp_ctx_t *ctx, uint16_t *s) { + cmp_object_t obj; + + if (!cmp_read_object(ctx, &obj)) + return false; + + if (obj.type != CMP_TYPE_UINT16) { + ctx->error = INVALID_TYPE_ERROR; + return false; + } + + *s = obj.as.u16; + return true; +} + +bool cmp_read_u32(cmp_ctx_t *ctx, uint32_t *i) { + cmp_object_t obj; + + if (!cmp_read_object(ctx, &obj)) + return false; + + if (obj.type != CMP_TYPE_UINT32) { + ctx->error = INVALID_TYPE_ERROR; + return false; + } + + *i = obj.as.u32; + return true; +} + +bool cmp_read_u64(cmp_ctx_t *ctx, uint64_t *l) { + cmp_object_t obj; + + if (!cmp_read_object(ctx, &obj)) + return false; + + if (obj.type != CMP_TYPE_UINT64) { + ctx->error = INVALID_TYPE_ERROR; + return false; + } + + *l = obj.as.u64; + return true; +} + +bool cmp_read_uchar(cmp_ctx_t *ctx, uint8_t *c) { + cmp_object_t obj; + + if (!cmp_read_object(ctx, &obj)) + return false; + + switch (obj.type) { + case CMP_TYPE_POSITIVE_FIXNUM: + case CMP_TYPE_UINT8: + *c = obj.as.u8; + return true; + case CMP_TYPE_NEGATIVE_FIXNUM: + case CMP_TYPE_SINT8: + if (obj.as.s8 >= 0) { + *c = obj.as.s8; + return true; + } + break; + default: + break; + } + + ctx->error = INVALID_TYPE_ERROR; + return false; +} + +bool cmp_read_ushort(cmp_ctx_t *ctx, uint16_t *s) { + cmp_object_t obj; + + if (!cmp_read_object(ctx, &obj)) + return false; + + switch (obj.type) { + case CMP_TYPE_POSITIVE_FIXNUM: + case CMP_TYPE_UINT8: + *s = obj.as.u8; + return true; + case CMP_TYPE_UINT16: + *s = obj.as.u16; + return true; + case CMP_TYPE_NEGATIVE_FIXNUM: + case CMP_TYPE_SINT8: + if (obj.as.s8 >= 0) { + *s = obj.as.s8; + return true; + } + break; + case CMP_TYPE_SINT16: + if (obj.as.s16 >= 0) { + *s = obj.as.s16; + return true; + } + break; + default: + break; + } + + ctx->error = INVALID_TYPE_ERROR; + return false; +} + +bool cmp_read_uint(cmp_ctx_t *ctx, uint32_t *i) { + cmp_object_t obj; + + if (!cmp_read_object(ctx, &obj)) + return false; + + switch (obj.type) { + case CMP_TYPE_POSITIVE_FIXNUM: + case CMP_TYPE_UINT8: + *i = obj.as.u8; + return true; + case CMP_TYPE_UINT16: + *i = obj.as.u16; + return true; + case CMP_TYPE_UINT32: + *i = obj.as.u32; + return true; + case CMP_TYPE_NEGATIVE_FIXNUM: + case CMP_TYPE_SINT8: + if (obj.as.s8 >= 0) { + *i = obj.as.s8; + return true; + } + break; + case CMP_TYPE_SINT16: + if (obj.as.s16 >= 0) { + *i = obj.as.s16; + return true; + } + break; + case CMP_TYPE_SINT32: + if (obj.as.s32 >= 0) { + *i = obj.as.s32; + return true; + } + break; + default: + break; + } + + ctx->error = INVALID_TYPE_ERROR; + return false; +} + +bool cmp_read_ulong(cmp_ctx_t *ctx, uint64_t *u) { + cmp_object_t obj; + + if (!cmp_read_object(ctx, &obj)) + return false; + + switch (obj.type) { + case CMP_TYPE_POSITIVE_FIXNUM: + case CMP_TYPE_UINT8: + *u = obj.as.u8; + return true; + case CMP_TYPE_UINT16: + *u = obj.as.u16; + return true; + case CMP_TYPE_UINT32: + *u = obj.as.u32; + return true; + case CMP_TYPE_UINT64: + *u = obj.as.u64; + return true; + case CMP_TYPE_NEGATIVE_FIXNUM: + case CMP_TYPE_SINT8: + if (obj.as.s8 >= 0) { + *u = obj.as.s8; + return true; + } + break; + case CMP_TYPE_SINT16: + if (obj.as.s16 >= 0) { + *u = obj.as.s16; + return true; + } + break; + case CMP_TYPE_SINT32: + if (obj.as.s32 >= 0) { + *u = obj.as.s32; + return true; + } + break; + case CMP_TYPE_SINT64: + if (obj.as.s64 >= 0) { + *u = obj.as.s64; + return true; + } + break; + default: + break; + } + + ctx->error = INVALID_TYPE_ERROR; + return false; +} + +bool cmp_read_uinteger(cmp_ctx_t *ctx, uint64_t *d) { + return cmp_read_ulong(ctx, d); +} + +bool cmp_read_float(cmp_ctx_t *ctx, float *f) { + cmp_object_t obj; + + if (!cmp_read_object(ctx, &obj)) + return false; + + if (obj.type != CMP_TYPE_FLOAT) { + ctx->error = INVALID_TYPE_ERROR; + return false; + } + + *f = obj.as.flt; + + return true; +} + +bool cmp_read_double(cmp_ctx_t *ctx, double *d) { + cmp_object_t obj; + + if (!cmp_read_object(ctx, &obj)) + return false; + + if (obj.type != CMP_TYPE_DOUBLE) { + ctx->error = INVALID_TYPE_ERROR; + return false; + } + + *d = obj.as.dbl; + + return true; +} + +bool cmp_read_decimal(cmp_ctx_t *ctx, double *d) { + cmp_object_t obj; + + if (!cmp_read_object(ctx, &obj)) + return false; + + switch (obj.type) { + case CMP_TYPE_FLOAT: + *d = (double)obj.as.flt; + return true; + case CMP_TYPE_DOUBLE: + *d = obj.as.dbl; + return true; + default: + ctx->error = INVALID_TYPE_ERROR; + return false; + } +} + +bool cmp_read_nil(cmp_ctx_t *ctx) { + cmp_object_t obj; + + if (!cmp_read_object(ctx, &obj)) + return false; + + if (obj.type == CMP_TYPE_NIL) + return true; + + ctx->error = INVALID_TYPE_ERROR; + return false; +} + +bool cmp_read_bool(cmp_ctx_t *ctx, bool *b) { + cmp_object_t obj; + + if (!cmp_read_object(ctx, &obj)) + return false; + + if (obj.type != CMP_TYPE_BOOLEAN) { + ctx->error = INVALID_TYPE_ERROR; + return false; + } + + if (obj.as.boolean) + *b = true; + else + *b = false; + + return true; +} + +bool cmp_read_bool_as_u8(cmp_ctx_t *ctx, uint8_t *b) { + cmp_object_t obj; + + if (!cmp_read_object(ctx, &obj)) + return false; + + if (obj.type != CMP_TYPE_BOOLEAN) { + ctx->error = INVALID_TYPE_ERROR; + return false; + } + + if (obj.as.boolean) + *b = 1; + else + *b = 0; + + return true; +} + +bool cmp_read_str_size(cmp_ctx_t *ctx, uint32_t *size) { + cmp_object_t obj; + + if (!cmp_read_object(ctx, &obj)) + return false; + + switch (obj.type) { + case CMP_TYPE_FIXSTR: + case CMP_TYPE_STR8: + case CMP_TYPE_STR16: + case CMP_TYPE_STR32: + *size = obj.as.str_size; + return true; + default: + ctx->error = INVALID_TYPE_ERROR; + return false; + } +} + +bool cmp_read_str(cmp_ctx_t *ctx, char *data, uint32_t *size) { + uint32_t str_size = 0; + + if (!cmp_read_str_size(ctx, &str_size)) + return false; + + if ((str_size + 1) > *size) { + *size = str_size; + ctx->error = STR_DATA_LENGTH_TOO_LONG_ERROR; + return false; + } + + if (!ctx->read(ctx, data, str_size)) { + ctx->error = DATA_READING_ERROR; + return false; + } + + data[str_size] = 0; + + *size = str_size; + return true; +} + +bool cmp_read_bin_size(cmp_ctx_t *ctx, uint32_t *size) { + cmp_object_t obj; + + if (!cmp_read_object(ctx, &obj)) + return false; + + switch (obj.type) { + case CMP_TYPE_BIN8: + case CMP_TYPE_BIN16: + case CMP_TYPE_BIN32: + *size = obj.as.bin_size; + return true; + default: + ctx->error = INVALID_TYPE_ERROR; + return false; + } +} + +bool cmp_read_bin(cmp_ctx_t *ctx, void *data, uint32_t *size) { + uint32_t bin_size = 0; + + if (!cmp_read_bin_size(ctx, &bin_size)) + return false; + + if (bin_size > *size) { + ctx->error = BIN_DATA_LENGTH_TOO_LONG_ERROR; + return false; + } + + if (!ctx->read(ctx, data, bin_size)) { + ctx->error = DATA_READING_ERROR; + return false; + } + + *size = bin_size; + return true; +} + +bool cmp_read_array(cmp_ctx_t *ctx, uint32_t *size) { + cmp_object_t obj; + + if (!cmp_read_object(ctx, &obj)) + return false; + + switch (obj.type) { + case CMP_TYPE_FIXARRAY: + case CMP_TYPE_ARRAY16: + case CMP_TYPE_ARRAY32: + *size = obj.as.array_size; + return true; + default: + ctx->error = INVALID_TYPE_ERROR; + return false; + } +} + +bool cmp_read_map(cmp_ctx_t *ctx, uint32_t *size) { + cmp_object_t obj; + + if (!cmp_read_object(ctx, &obj)) + return false; + + switch (obj.type) { + case CMP_TYPE_FIXMAP: + case CMP_TYPE_MAP16: + case CMP_TYPE_MAP32: + *size = obj.as.map_size; + return true; + default: + ctx->error = INVALID_TYPE_ERROR; + return false; + } +} + +bool cmp_read_fixext1_marker(cmp_ctx_t *ctx, int8_t *type) { + cmp_object_t obj; + + if (!cmp_read_object(ctx, &obj)) + return false; + + if (obj.type != CMP_TYPE_FIXEXT1) { + ctx->error = INVALID_TYPE_ERROR; + return false; + } + + *type = obj.as.ext.type; + return true; +} + +bool cmp_read_fixext1(cmp_ctx_t *ctx, int8_t *type, void *data) { + if (!cmp_read_fixext1_marker(ctx, type)) + return false; + + if (ctx->read(ctx, data, 1)) + return true; + + ctx->error = DATA_READING_ERROR; + return false; +} + +bool cmp_read_fixext2_marker(cmp_ctx_t *ctx, int8_t *type) { + cmp_object_t obj; + + if (!cmp_read_object(ctx, &obj)) + return false; + + if (obj.type != CMP_TYPE_FIXEXT2) { + ctx->error = INVALID_TYPE_ERROR; + return false; + } + + *type = obj.as.ext.type; + return true; +} + +bool cmp_read_fixext2(cmp_ctx_t *ctx, int8_t *type, void *data) { + if (!cmp_read_fixext2_marker(ctx, type)) + return false; + + if (ctx->read(ctx, data, 2)) + return true; + + ctx->error = DATA_READING_ERROR; + return false; +} + +bool cmp_read_fixext4_marker(cmp_ctx_t *ctx, int8_t *type) { + cmp_object_t obj; + + if (!cmp_read_object(ctx, &obj)) + return false; + + if (obj.type != CMP_TYPE_FIXEXT4) { + ctx->error = INVALID_TYPE_ERROR; + return false; + } + + *type = obj.as.ext.type; + return true; +} + +bool cmp_read_fixext4(cmp_ctx_t *ctx, int8_t *type, void *data) { + if (!cmp_read_fixext4_marker(ctx, type)) + return false; + + if (ctx->read(ctx, data, 4)) + return true; + + ctx->error = DATA_READING_ERROR; + return false; +} + +bool cmp_read_fixext8_marker(cmp_ctx_t *ctx, int8_t *type) { + cmp_object_t obj; + + if (!cmp_read_object(ctx, &obj)) + return false; + + if (obj.type != CMP_TYPE_FIXEXT8) { + ctx->error = INVALID_TYPE_ERROR; + return false; + } + + *type = obj.as.ext.type; + return true; +} + +bool cmp_read_fixext8(cmp_ctx_t *ctx, int8_t *type, void *data) { + if (!cmp_read_fixext8_marker(ctx, type)) + return false; + + if (ctx->read(ctx, data, 8)) + return true; + + ctx->error = DATA_READING_ERROR; + return false; +} + +bool cmp_read_fixext16_marker(cmp_ctx_t *ctx, int8_t *type) { + cmp_object_t obj; + + if (!cmp_read_object(ctx, &obj)) + return false; + + if (obj.type != CMP_TYPE_FIXEXT16) { + ctx->error = INVALID_TYPE_ERROR; + return false; + } + + *type = obj.as.ext.type; + return true; +} + +bool cmp_read_fixext16(cmp_ctx_t *ctx, int8_t *type, void *data) { + if (!cmp_read_fixext16_marker(ctx, type)) + return false; + + if (ctx->read(ctx, data, 16)) + return true; + + ctx->error = DATA_READING_ERROR; + return false; +} + +bool cmp_read_ext8_marker(cmp_ctx_t *ctx, int8_t *type, uint8_t *size) { + cmp_object_t obj; + + if (!cmp_read_object(ctx, &obj)) + return false; + + if (obj.type != CMP_TYPE_EXT8) { + ctx->error = INVALID_TYPE_ERROR; + return false; + } + + *type = obj.as.ext.type; + *size = (uint8_t)obj.as.ext.size; + + return true; +} + +bool cmp_read_ext8(cmp_ctx_t *ctx, int8_t *type, uint8_t *size, void *data) { + if (!cmp_read_ext8_marker(ctx, type, size)) + return false; + + if (ctx->read(ctx, data, *size)) + return true; + + ctx->error = DATA_READING_ERROR; + return false; +} + +bool cmp_read_ext16_marker(cmp_ctx_t *ctx, int8_t *type, uint16_t *size) { + cmp_object_t obj; + + if (!cmp_read_object(ctx, &obj)) + return false; + + if (obj.type != CMP_TYPE_EXT16) { + ctx->error = INVALID_TYPE_ERROR; + return false; + } + + *type = obj.as.ext.type; + *size = (uint16_t)obj.as.ext.size; + + return true; +} + +bool cmp_read_ext16(cmp_ctx_t *ctx, int8_t *type, uint16_t *size, void *data) { + if (!cmp_read_ext16_marker(ctx, type, size)) + return false; + + if (ctx->read(ctx, data, *size)) + return true; + + ctx->error = DATA_READING_ERROR; + return false; +} + +bool cmp_read_ext32_marker(cmp_ctx_t *ctx, int8_t *type, uint32_t *size) { + cmp_object_t obj; + + if (!cmp_read_object(ctx, &obj)) + return false; + + if (obj.type != CMP_TYPE_EXT32) { + ctx->error = INVALID_TYPE_ERROR; + return false; + } + + *type = obj.as.ext.type; + *size = obj.as.ext.size; + + return true; +} + +bool cmp_read_ext32(cmp_ctx_t *ctx, int8_t *type, uint32_t *size, void *data) { + if (!cmp_read_ext32_marker(ctx, type, size)) + return false; + + if (ctx->read(ctx, data, *size)) + return true; + + ctx->error = DATA_READING_ERROR; + return false; +} + +bool cmp_read_ext_marker(cmp_ctx_t *ctx, int8_t *type, uint32_t *size) { + cmp_object_t obj; + + if (!cmp_read_object(ctx, &obj)) + return false; + + switch (obj.type) { + case CMP_TYPE_FIXEXT1: + case CMP_TYPE_FIXEXT2: + case CMP_TYPE_FIXEXT4: + case CMP_TYPE_FIXEXT8: + case CMP_TYPE_FIXEXT16: + case CMP_TYPE_EXT8: + case CMP_TYPE_EXT16: + case CMP_TYPE_EXT32: + *type = obj.as.ext.type; + *size = obj.as.ext.size; + return true; + default: + ctx->error = INVALID_TYPE_ERROR; + return false; + } +} + +bool cmp_read_ext(cmp_ctx_t *ctx, int8_t *type, uint32_t *size, void *data) { + if (!cmp_read_ext_marker(ctx, type, size)) + return false; + + if (ctx->read(ctx, data, *size)) + return true; + + ctx->error = DATA_READING_ERROR; + return false; +} + +bool cmp_read_object(cmp_ctx_t *ctx, cmp_object_t *obj) { + uint8_t type_marker = 0; + + if (!read_type_marker(ctx, &type_marker)) + return false; + + if (!type_marker_to_cmp_type(type_marker, &obj->type)) { + ctx->error = INVALID_TYPE_ERROR; + return false; + } + + return read_obj_data(ctx, type_marker, obj); +} + +bool cmp_skip_object(cmp_ctx_t *ctx, cmp_object_t *obj) { + return cmp_skip_object_limit(ctx, obj, 0); +} + +bool cmp_skip_object_limit(cmp_ctx_t *ctx, cmp_object_t *obj, uint32_t limit) { + size_t element_count = 1; + uint32_t depth = 0; + + while (element_count) { + uint8_t type_marker = 0; + uint8_t cmp_type; + uint32_t size = 0; + + if (!read_type_marker(ctx, &type_marker)) { + return false; + } + + if (!type_marker_to_cmp_type(type_marker, &cmp_type)) { + ctx->error = INVALID_TYPE_ERROR; + return false; + } + + switch (cmp_type) { + case CMP_TYPE_FIXARRAY: + case CMP_TYPE_ARRAY16: + case CMP_TYPE_ARRAY32: + case CMP_TYPE_FIXMAP: + case CMP_TYPE_MAP16: + case CMP_TYPE_MAP32: + depth++; + + if (depth > limit) { + obj->type = cmp_type; + + if (!read_obj_data(ctx, type_marker, obj)) { + return false; + } + + ctx->error = SKIP_DEPTH_LIMIT_EXCEEDED_ERROR; + + return false; + } + + break; + default: + if (!read_type_size(ctx, type_marker, cmp_type, &size)) { + return false; + } + + if (size) { + switch (cmp_type) { + case CMP_TYPE_FIXEXT1: + case CMP_TYPE_FIXEXT2: + case CMP_TYPE_FIXEXT4: + case CMP_TYPE_FIXEXT8: + case CMP_TYPE_FIXEXT16: + case CMP_TYPE_EXT8: + case CMP_TYPE_EXT16: + case CMP_TYPE_EXT32: + size++; + default: + break; + } + + skip_bytes(ctx, size); + } + } + + element_count--; + + switch (cmp_type) { + case CMP_TYPE_FIXARRAY: + case CMP_TYPE_ARRAY16: + case CMP_TYPE_ARRAY32: + if (!read_type_size(ctx, type_marker, cmp_type, &size)) { + return false; + } + element_count += size; + break; + case CMP_TYPE_FIXMAP: + case CMP_TYPE_MAP16: + case CMP_TYPE_MAP32: + if (!read_type_size(ctx, type_marker, cmp_type, &size)) { + return false; + } + element_count += ((size_t)size) * 2; + break; + default: + break; + } + } + + return true; +} + +bool cmp_skip_object_no_limit(cmp_ctx_t *ctx) { + size_t element_count = 1; + + while (element_count) { + uint8_t type_marker = 0; + uint8_t cmp_type = 0; + uint32_t size = 0; + + if (!read_type_marker(ctx, &type_marker)) { + return false; + } + + if (!type_marker_to_cmp_type(type_marker, &cmp_type)) { + ctx->error = INVALID_TYPE_ERROR; + return false; + } + + switch (cmp_type) { + case CMP_TYPE_FIXARRAY: + case CMP_TYPE_ARRAY16: + case CMP_TYPE_ARRAY32: + case CMP_TYPE_FIXMAP: + case CMP_TYPE_MAP16: + case CMP_TYPE_MAP32: + break; + default: + if (!read_type_size(ctx, type_marker, cmp_type, &size)) { + return false; + } + + if (size) { + switch (cmp_type) { + case CMP_TYPE_FIXEXT1: + case CMP_TYPE_FIXEXT2: + case CMP_TYPE_FIXEXT4: + case CMP_TYPE_FIXEXT8: + case CMP_TYPE_FIXEXT16: + case CMP_TYPE_EXT8: + case CMP_TYPE_EXT16: + case CMP_TYPE_EXT32: + size++; + default: + break; + } + + skip_bytes(ctx, size); + } + } + + element_count--; + + switch (cmp_type) { + case CMP_TYPE_FIXARRAY: + case CMP_TYPE_ARRAY16: + case CMP_TYPE_ARRAY32: + if (!read_type_size(ctx, type_marker, cmp_type, &size)) { + return false; + } + element_count += size; + break; + case CMP_TYPE_FIXMAP: + case CMP_TYPE_MAP16: + case CMP_TYPE_MAP32: + if (!read_type_size(ctx, type_marker, cmp_type, &size)) { + return false; + } + element_count += ((size_t)size) * 2; + break; + default: + break; + } + } + + return true; +} + +bool cmp_object_is_char(cmp_object_t *obj) { + switch (obj->type) { + case CMP_TYPE_NEGATIVE_FIXNUM: + case CMP_TYPE_SINT8: + return true; + default: + return false; + } +} + +bool cmp_object_is_short(cmp_object_t *obj) { + switch (obj->type) { + case CMP_TYPE_NEGATIVE_FIXNUM: + case CMP_TYPE_SINT8: + case CMP_TYPE_SINT16: + return true; + default: + return false; + } +} + +bool cmp_object_is_int(cmp_object_t *obj) { + switch (obj->type) { + case CMP_TYPE_NEGATIVE_FIXNUM: + case CMP_TYPE_SINT8: + case CMP_TYPE_SINT16: + case CMP_TYPE_SINT32: + return true; + default: + return false; + } +} + +bool cmp_object_is_long(cmp_object_t *obj) { + switch (obj->type) { + case CMP_TYPE_NEGATIVE_FIXNUM: + case CMP_TYPE_SINT8: + case CMP_TYPE_SINT16: + case CMP_TYPE_SINT32: + case CMP_TYPE_SINT64: + return true; + default: + return false; + } +} + +bool cmp_object_is_sinteger(cmp_object_t *obj) { + return cmp_object_is_long(obj); +} + +bool cmp_object_is_uchar(cmp_object_t *obj) { + switch (obj->type) { + case CMP_TYPE_POSITIVE_FIXNUM: + case CMP_TYPE_UINT8: + return true; + default: + return false; + } +} + +bool cmp_object_is_ushort(cmp_object_t *obj) { + switch (obj->type) { + case CMP_TYPE_POSITIVE_FIXNUM: + case CMP_TYPE_UINT8: + return true; + case CMP_TYPE_UINT16: + return true; + default: + return false; + } +} + +bool cmp_object_is_uint(cmp_object_t *obj) { + switch (obj->type) { + case CMP_TYPE_POSITIVE_FIXNUM: + case CMP_TYPE_UINT8: + case CMP_TYPE_UINT16: + case CMP_TYPE_UINT32: + return true; + default: + return false; + } +} + +bool cmp_object_is_ulong(cmp_object_t *obj) { + switch (obj->type) { + case CMP_TYPE_POSITIVE_FIXNUM: + case CMP_TYPE_UINT8: + case CMP_TYPE_UINT16: + case CMP_TYPE_UINT32: + case CMP_TYPE_UINT64: + return true; + default: + return false; + } +} + +bool cmp_object_is_uinteger(cmp_object_t *obj) { + return cmp_object_is_ulong(obj); +} + +bool cmp_object_is_float(cmp_object_t *obj) { + if (obj->type == CMP_TYPE_FLOAT) + return true; + + return false; +} + +bool cmp_object_is_double(cmp_object_t *obj) { + if (obj->type == CMP_TYPE_DOUBLE) + return true; + + return false; +} + +bool cmp_object_is_nil(cmp_object_t *obj) { + if (obj->type == CMP_TYPE_NIL) + return true; + + return false; +} + +bool cmp_object_is_bool(cmp_object_t *obj) { + if (obj->type == CMP_TYPE_BOOLEAN) + return true; + + return false; +} + +bool cmp_object_is_str(cmp_object_t *obj) { + switch (obj->type) { + case CMP_TYPE_FIXSTR: + case CMP_TYPE_STR8: + case CMP_TYPE_STR16: + case CMP_TYPE_STR32: + return true; + default: + return false; + } +} + +bool cmp_object_is_bin(cmp_object_t *obj) { + switch (obj->type) { + case CMP_TYPE_BIN8: + case CMP_TYPE_BIN16: + case CMP_TYPE_BIN32: + return true; + default: + return false; + } +} + +bool cmp_object_is_array(cmp_object_t *obj) { + switch (obj->type) { + case CMP_TYPE_FIXARRAY: + case CMP_TYPE_ARRAY16: + case CMP_TYPE_ARRAY32: + return true; + default: + return false; + } +} + +bool cmp_object_is_map(cmp_object_t *obj) { + switch (obj->type) { + case CMP_TYPE_FIXMAP: + case CMP_TYPE_MAP16: + case CMP_TYPE_MAP32: + return true; + default: + return false; + } +} + +bool cmp_object_is_ext(cmp_object_t *obj) { + switch (obj->type) { + case CMP_TYPE_FIXEXT1: + case CMP_TYPE_FIXEXT2: + case CMP_TYPE_FIXEXT4: + case CMP_TYPE_FIXEXT8: + case CMP_TYPE_FIXEXT16: + case CMP_TYPE_EXT8: + case CMP_TYPE_EXT16: + case CMP_TYPE_EXT32: + return true; + default: + return false; + } +} + +bool cmp_object_as_char(cmp_object_t *obj, int8_t *c) { + switch (obj->type) { + case CMP_TYPE_POSITIVE_FIXNUM: + case CMP_TYPE_NEGATIVE_FIXNUM: + case CMP_TYPE_SINT8: + *c = obj->as.s8; + return true; + case CMP_TYPE_UINT8: + if (obj->as.u8 <= 127) { + *c = obj->as.s8; + return true; + } + else { + return false; + } + default: + return false; + } +} + +bool cmp_object_as_short(cmp_object_t *obj, int16_t *s) { + switch (obj->type) { + case CMP_TYPE_POSITIVE_FIXNUM: + case CMP_TYPE_NEGATIVE_FIXNUM: + case CMP_TYPE_SINT8: + *s = obj->as.s8; + return true; + case CMP_TYPE_UINT8: + *s = obj->as.u8; + return true; + case CMP_TYPE_SINT16: + *s = obj->as.s16; + return true; + case CMP_TYPE_UINT16: + if (obj->as.u16 <= 32767) { + *s = obj->as.u16; + return true; + } + else { + return false; + } + default: + return false; + } +} + +bool cmp_object_as_int(cmp_object_t *obj, int32_t *i) { + switch (obj->type) { + case CMP_TYPE_POSITIVE_FIXNUM: + case CMP_TYPE_NEGATIVE_FIXNUM: + case CMP_TYPE_SINT8: + *i = obj->as.s8; + return true; + case CMP_TYPE_UINT8: + *i = obj->as.u8; + return true; + case CMP_TYPE_SINT16: + *i = obj->as.s16; + return true; + case CMP_TYPE_UINT16: + *i = obj->as.u16; + return true; + case CMP_TYPE_SINT32: + *i = obj->as.s32; + return true; + case CMP_TYPE_UINT32: + if (obj->as.u32 <= 2147483647) { + *i = obj->as.u32; + return true; + } + else { + return false; + } + default: + return false; + } +} + +bool cmp_object_as_long(cmp_object_t *obj, int64_t *d) { + switch (obj->type) { + case CMP_TYPE_POSITIVE_FIXNUM: + case CMP_TYPE_NEGATIVE_FIXNUM: + case CMP_TYPE_SINT8: + *d = obj->as.s8; + return true; + case CMP_TYPE_UINT8: + *d = obj->as.u8; + return true; + case CMP_TYPE_SINT16: + *d = obj->as.s16; + return true; + case CMP_TYPE_UINT16: + *d = obj->as.u16; + return true; + case CMP_TYPE_SINT32: + *d = obj->as.s32; + return true; + case CMP_TYPE_UINT32: + *d = obj->as.u32; + return true; + case CMP_TYPE_SINT64: + *d = obj->as.s64; + return true; + case CMP_TYPE_UINT64: + if (obj->as.u64 <= 9223372036854775807) { + *d = obj->as.u64; + return true; + } + else { + return false; + } + default: + return false; + } +} + +bool cmp_object_as_sinteger(cmp_object_t *obj, int64_t *d) { + return cmp_object_as_long(obj, d); +} + +bool cmp_object_as_uchar(cmp_object_t *obj, uint8_t *c) { + switch (obj->type) { + case CMP_TYPE_POSITIVE_FIXNUM: + case CMP_TYPE_UINT8: + *c = obj->as.u8; + return true; + default: + return false; + } +} + +bool cmp_object_as_ushort(cmp_object_t *obj, uint16_t *s) { + switch (obj->type) { + case CMP_TYPE_POSITIVE_FIXNUM: + case CMP_TYPE_UINT8: + *s = obj->as.u8; + return true; + case CMP_TYPE_UINT16: + *s = obj->as.u16; + return true; + default: + return false; + } +} + +bool cmp_object_as_uint(cmp_object_t *obj, uint32_t *i) { + switch (obj->type) { + case CMP_TYPE_POSITIVE_FIXNUM: + case CMP_TYPE_UINT8: + *i = obj->as.u8; + return true; + case CMP_TYPE_UINT16: + *i = obj->as.u16; + return true; + case CMP_TYPE_UINT32: + *i = obj->as.u32; + return true; + default: + return false; + } +} + +bool cmp_object_as_ulong(cmp_object_t *obj, uint64_t *u) { + switch (obj->type) { + case CMP_TYPE_POSITIVE_FIXNUM: + case CMP_TYPE_UINT8: + *u = obj->as.u8; + return true; + case CMP_TYPE_UINT16: + *u = obj->as.u16; + return true; + case CMP_TYPE_UINT32: + *u = obj->as.u32; + return true; + case CMP_TYPE_UINT64: + *u = obj->as.u64; + return true; + default: + return false; + } +} + +bool cmp_object_as_uinteger(cmp_object_t *obj, uint64_t *d) { + return cmp_object_as_ulong(obj, d); +} + +bool cmp_object_as_float(cmp_object_t *obj, float *f) { + if (obj->type == CMP_TYPE_FLOAT) { + *f = obj->as.flt; + return true; + } + + return false; +} + +bool cmp_object_as_double(cmp_object_t *obj, double *d) { + if (obj->type == CMP_TYPE_DOUBLE) { + *d = obj->as.dbl; + return true; + } + + return false; +} + +bool cmp_object_as_bool(cmp_object_t *obj, bool *b) { + if (obj->type == CMP_TYPE_BOOLEAN) { + if (obj->as.boolean) + *b = true; + else + *b = false; + + return true; + } + + return false; +} + +bool cmp_object_as_str(cmp_object_t *obj, uint32_t *size) { + switch (obj->type) { + case CMP_TYPE_FIXSTR: + case CMP_TYPE_STR8: + case CMP_TYPE_STR16: + case CMP_TYPE_STR32: + *size = obj->as.str_size; + return true; + default: + return false; + } +} + +bool cmp_object_as_bin(cmp_object_t *obj, uint32_t *size) { + switch (obj->type) { + case CMP_TYPE_BIN8: + case CMP_TYPE_BIN16: + case CMP_TYPE_BIN32: + *size = obj->as.bin_size; + return true; + default: + return false; + } +} + +bool cmp_object_as_array(cmp_object_t *obj, uint32_t *size) { + switch (obj->type) { + case CMP_TYPE_FIXARRAY: + case CMP_TYPE_ARRAY16: + case CMP_TYPE_ARRAY32: + *size = obj->as.array_size; + return true; + default: + return false; + } +} + +bool cmp_object_as_map(cmp_object_t *obj, uint32_t *size) { + switch (obj->type) { + case CMP_TYPE_FIXMAP: + case CMP_TYPE_MAP16: + case CMP_TYPE_MAP32: + *size = obj->as.map_size; + return true; + default: + return false; + } +} + +bool cmp_object_as_ext(cmp_object_t *obj, int8_t *type, uint32_t *size) { + switch (obj->type) { + case CMP_TYPE_FIXEXT1: + case CMP_TYPE_FIXEXT2: + case CMP_TYPE_FIXEXT4: + case CMP_TYPE_FIXEXT8: + case CMP_TYPE_FIXEXT16: + case CMP_TYPE_EXT8: + case CMP_TYPE_EXT16: + case CMP_TYPE_EXT32: + *type = obj->as.ext.type; + *size = obj->as.ext.size; + return true; + default: + return false; + } +} + +bool cmp_object_to_str(cmp_ctx_t *ctx, cmp_object_t *obj, char *data, + uint32_t buf_size) { + uint32_t str_size = 0; + + switch (obj->type) { + case CMP_TYPE_FIXSTR: + case CMP_TYPE_STR8: + case CMP_TYPE_STR16: + case CMP_TYPE_STR32: + str_size = obj->as.str_size; + if ((str_size + 1) > buf_size) { + ctx->error = STR_DATA_LENGTH_TOO_LONG_ERROR; + return false; + } + + if (!ctx->read(ctx, data, str_size)) { + ctx->error = DATA_READING_ERROR; + return false; + } + + data[str_size] = 0; + return true; + default: + return false; + } +} + +bool cmp_object_to_bin(cmp_ctx_t *ctx, cmp_object_t *obj, void *data, + uint32_t buf_size) { + uint32_t bin_size = 0; + + switch (obj->type) { + case CMP_TYPE_BIN8: + case CMP_TYPE_BIN16: + case CMP_TYPE_BIN32: + bin_size = obj->as.bin_size; + if (bin_size > buf_size) { + ctx->error = BIN_DATA_LENGTH_TOO_LONG_ERROR; + return false; + } + + if (!ctx->read(ctx, data, bin_size)) { + ctx->error = DATA_READING_ERROR; + return false; + } + return true; + default: + return false; + } +} + +/* vi: set et ts=2 sw=2: */ + diff --git a/cmp.h b/cmp.h new file mode 100644 index 0000000..c78b37b --- /dev/null +++ b/cmp.h @@ -0,0 +1,526 @@ +/* +The MIT License (MIT) + +Copyright (c) 2017 Charles Gunyon + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +*/ + +#ifndef CMP_H__ +#define CMP_H__ + +struct cmp_ctx_s; + +typedef bool (*cmp_reader)(struct cmp_ctx_s *ctx, void *data, size_t limit); +typedef bool (*cmp_skipper)(struct cmp_ctx_s *ctx, size_t count); +typedef size_t (*cmp_writer)(struct cmp_ctx_s *ctx, const void *data, + size_t count); + +enum { + CMP_TYPE_POSITIVE_FIXNUM, /* 0 */ + CMP_TYPE_FIXMAP, /* 1 */ + CMP_TYPE_FIXARRAY, /* 2 */ + CMP_TYPE_FIXSTR, /* 3 */ + CMP_TYPE_NIL, /* 4 */ + CMP_TYPE_BOOLEAN, /* 5 */ + CMP_TYPE_BIN8, /* 6 */ + CMP_TYPE_BIN16, /* 7 */ + CMP_TYPE_BIN32, /* 8 */ + CMP_TYPE_EXT8, /* 9 */ + CMP_TYPE_EXT16, /* 10 */ + CMP_TYPE_EXT32, /* 11 */ + CMP_TYPE_FLOAT, /* 12 */ + CMP_TYPE_DOUBLE, /* 13 */ + CMP_TYPE_UINT8, /* 14 */ + CMP_TYPE_UINT16, /* 15 */ + CMP_TYPE_UINT32, /* 16 */ + CMP_TYPE_UINT64, /* 17 */ + CMP_TYPE_SINT8, /* 18 */ + CMP_TYPE_SINT16, /* 19 */ + CMP_TYPE_SINT32, /* 20 */ + CMP_TYPE_SINT64, /* 21 */ + CMP_TYPE_FIXEXT1, /* 22 */ + CMP_TYPE_FIXEXT2, /* 23 */ + CMP_TYPE_FIXEXT4, /* 24 */ + CMP_TYPE_FIXEXT8, /* 25 */ + CMP_TYPE_FIXEXT16, /* 26 */ + CMP_TYPE_STR8, /* 27 */ + CMP_TYPE_STR16, /* 28 */ + CMP_TYPE_STR32, /* 29 */ + CMP_TYPE_ARRAY16, /* 30 */ + CMP_TYPE_ARRAY32, /* 31 */ + CMP_TYPE_MAP16, /* 32 */ + CMP_TYPE_MAP32, /* 33 */ + CMP_TYPE_NEGATIVE_FIXNUM /* 34 */ +}; + +typedef struct cmp_ext_s { + int8_t type; + uint32_t size; +} cmp_ext_t; + +union cmp_object_data_u { + bool boolean; + uint8_t u8; + uint16_t u16; + uint32_t u32; + uint64_t u64; + int8_t s8; + int16_t s16; + int32_t s32; + int64_t s64; + float flt; + double dbl; + uint32_t array_size; + uint32_t map_size; + uint32_t str_size; + uint32_t bin_size; + cmp_ext_t ext; +}; + +typedef struct cmp_ctx_s { + uint8_t error; + void *buf; + cmp_reader read; + cmp_skipper skip; + cmp_writer write; +} cmp_ctx_t; + +typedef struct cmp_object_s { + uint8_t type; + union cmp_object_data_u as; +} cmp_object_t; + +#ifdef __cplusplus +extern "C" { +#endif + +/* + * ============================================================================ + * === Main API + * ============================================================================ + */ + +/* + * Initializes a CMP context + * + * If you don't intend to read, `read` may be NULL, but calling `*read*` + * functions will crash; there is no check. + * + * `skip` may be NULL, in which case skipping functions will use `read`. + * + * If you don't intend to write, `write` may be NULL, but calling `*write*` + * functions will crash; there is no check. + */ +void cmp_init(cmp_ctx_t *ctx, void *buf, cmp_reader read, + cmp_skipper skip, + cmp_writer write); + +/* Returns CMP's version */ +uint32_t cmp_version(void); + +/* Returns the MessagePack version employed by CMP */ +uint32_t cmp_mp_version(void); + +/* Returns a string description of a CMP context's error */ +const char* cmp_strerror(cmp_ctx_t *ctx); + +/* Writes a signed integer to the backend */ +bool cmp_write_integer(cmp_ctx_t *ctx, int64_t d); + +/* Writes an unsigned integer to the backend */ +bool cmp_write_uinteger(cmp_ctx_t *ctx, uint64_t u); + +/* + * Writes a floating-point value (either single or double-precision) to the + * backend + */ +bool cmp_write_decimal(cmp_ctx_t *ctx, double d); + +/* Writes NULL to the backend */ +bool cmp_write_nil(cmp_ctx_t *ctx); + +/* Writes true to the backend */ +bool cmp_write_true(cmp_ctx_t *ctx); + +/* Writes false to the backend */ +bool cmp_write_false(cmp_ctx_t *ctx); + +/* Writes a boolean value to the backend */ +bool cmp_write_bool(cmp_ctx_t *ctx, bool b); + +/* + * Writes an unsigned char's value to the backend as a boolean. This is useful + * if you are using a different boolean type in your application. + */ +bool cmp_write_u8_as_bool(cmp_ctx_t *ctx, uint8_t b); + +/* + * Writes a string to the backend; according to the MessagePack spec, this must + * be encoded using UTF-8, but CMP leaves that job up to the programmer. + */ +bool cmp_write_str(cmp_ctx_t *ctx, const char *data, uint32_t size); + +/* + * Writes a string to the backend. This avoids using the STR8 marker, which + * is unsupported by MessagePack v4, the version implemented by many other + * MessagePack libraries. No encoding is assumed in this case, not that it + * matters. + */ +bool cmp_write_str_v4(cmp_ctx_t *ctx, const char *data, uint32_t size); + +/* + * Writes the string marker to the backend. This is useful if you are writing + * data in chunks instead of a single shot. + */ +bool cmp_write_str_marker(cmp_ctx_t *ctx, uint32_t size); + +/* + * Writes the string marker to the backend. This is useful if you are writing + * data in chunks instead of a single shot. This avoids using the STR8 + * marker, which is unsupported by MessagePack v4, the version implemented by + * many other MessagePack libraries. No encoding is assumed in this case, not + * that it matters. + */ +bool cmp_write_str_marker_v4(cmp_ctx_t *ctx, uint32_t size); + +/* Writes binary data to the backend */ +bool cmp_write_bin(cmp_ctx_t *ctx, const void *data, uint32_t size); + +/* + * Writes the binary data marker to the backend. This is useful if you are + * writing data in chunks instead of a single shot. + */ +bool cmp_write_bin_marker(cmp_ctx_t *ctx, uint32_t size); + +/* Writes an array to the backend. */ +bool cmp_write_array(cmp_ctx_t *ctx, uint32_t size); + +/* Writes a map to the backend. */ +bool cmp_write_map(cmp_ctx_t *ctx, uint32_t size); + +/* Writes an extended type to the backend */ +bool cmp_write_ext(cmp_ctx_t *ctx, int8_t type, uint32_t size, + const void *data); + +/* + * Writes the extended type marker to the backend. This is useful if you want + * to write the type's data in chunks instead of a single shot. + */ +bool cmp_write_ext_marker(cmp_ctx_t *ctx, int8_t type, uint32_t size); + +/* Writes an object to the backend */ +bool cmp_write_object(cmp_ctx_t *ctx, cmp_object_t *obj); + +/* + * Writes an object to the backend. This avoids using the STR8 marker, which + * is unsupported by MessagePack v4, the version implemented by many other + * MessagePack libraries. + */ +bool cmp_write_object_v4(cmp_ctx_t *ctx, cmp_object_t *obj); + +/* Reads a signed integer that fits inside a signed char */ +bool cmp_read_char(cmp_ctx_t *ctx, int8_t *c); + +/* Reads a signed integer that fits inside a signed short */ +bool cmp_read_short(cmp_ctx_t *ctx, int16_t *s); + +/* Reads a signed integer that fits inside a signed int */ +bool cmp_read_int(cmp_ctx_t *ctx, int32_t *i); + +/* Reads a signed integer that fits inside a signed long */ +bool cmp_read_long(cmp_ctx_t *ctx, int64_t *d); + +/* Reads a signed integer */ +bool cmp_read_integer(cmp_ctx_t *ctx, int64_t *d); + +/* Reads an unsigned integer that fits inside an unsigned char */ +bool cmp_read_uchar(cmp_ctx_t *ctx, uint8_t *c); + +/* Reads an unsigned integer that fits inside an unsigned short */ +bool cmp_read_ushort(cmp_ctx_t *ctx, uint16_t *s); + +/* Reads an unsigned integer that fits inside an unsigned int */ +bool cmp_read_uint(cmp_ctx_t *ctx, uint32_t *i); + +/* Reads an unsigned integer that fits inside an unsigned long */ +bool cmp_read_ulong(cmp_ctx_t *ctx, uint64_t *u); + +/* Reads an unsigned integer */ +bool cmp_read_uinteger(cmp_ctx_t *ctx, uint64_t *u); + +/* + * Reads a floating point value (either single or double-precision) from the + * backend + */ +bool cmp_read_decimal(cmp_ctx_t *ctx, double *d); + +/* "Reads" (more like "skips") a NULL value from the backend */ +bool cmp_read_nil(cmp_ctx_t *ctx); + +/* Reads a boolean from the backend */ +bool cmp_read_bool(cmp_ctx_t *ctx, bool *b); + +/* + * Reads a boolean as an unsigned char from the backend; this is useful if your + * application uses a different boolean type. + */ +bool cmp_read_bool_as_u8(cmp_ctx_t *ctx, uint8_t *b); + +/* Reads a string's size from the backend */ +bool cmp_read_str_size(cmp_ctx_t *ctx, uint32_t *size); + +/* + * Reads a string from the backend; according to the spec, the string's data + * ought to be encoded using UTF-8, + */ +bool cmp_read_str(cmp_ctx_t *ctx, char *data, uint32_t *size); + +/* Reads the size of packed binary data from the backend */ +bool cmp_read_bin_size(cmp_ctx_t *ctx, uint32_t *size); + +/* Reads packed binary data from the backend */ +bool cmp_read_bin(cmp_ctx_t *ctx, void *data, uint32_t *size); + +/* Reads an array from the backend */ +bool cmp_read_array(cmp_ctx_t *ctx, uint32_t *size); + +/* Reads a map from the backend */ +bool cmp_read_map(cmp_ctx_t *ctx, uint32_t *size); + +/* Reads the extended type's marker from the backend */ +bool cmp_read_ext_marker(cmp_ctx_t *ctx, int8_t *type, uint32_t *size); + +/* Reads an extended type from the backend */ +bool cmp_read_ext(cmp_ctx_t *ctx, int8_t *type, uint32_t *size, void *data); + +/* Reads an object from the backend */ +bool cmp_read_object(cmp_ctx_t *ctx, cmp_object_t *obj); + +/* + * Skips the next object from the backend. If that object is an array or map, + * this function will: + * - If `obj` is not `NULL`, fill in `obj` with that object + * - Set `ctx->error` to `SKIP_DEPTH_LIMIT_EXCEEDED_ERROR` + * - Return `false` + * Otherwise: + * - (Don't touch `obj`) + * - Return `true` + */ +bool cmp_skip_object(cmp_ctx_t *ctx, cmp_object_t *obj); + +/* + * This is similar to `cmp_skip_object`, except it tolerates up to `limit` + * levels of nesting. For example, in order to skip an array that contains a + * map, call `cmp_skip_object_limit(ctx, &obj, 2)`. Or in other words, + * `cmp_skip_object(ctx, &obj)` acts similarly to `cmp_skip_object_limit(ctx, + * &obj, 0)` + * + * Specifically, `limit` refers to depth, not breadth. So in order to skip an + * array that contains two arrays that each contain 3 strings, you would call + * `cmp_skip_object_limit(ctx, &obj, 2). In order to skip an array that + * contains 4 arrays that each contain 1 string, you would still call + * `cmp_skip_object_limit(ctx, &obj, 2). + */ +bool cmp_skip_object_limit(cmp_ctx_t *ctx, cmp_object_t *obj, uint32_t limit); + +/* + * This is similar to `cmp_skip_object`, except it will continually skip + * nested data structures. + * + * WARNING: This can cause your application to spend an unbounded amount of + * time reading nested data structures. Unless you completely trust + * the data source, you should strongly consider `cmp_skip_object` or + * `cmp_skip_object_limit`. + */ +bool cmp_skip_object_no_limit(cmp_ctx_t *ctx); + +/* + * ============================================================================ + * === Specific API + * ============================================================================ + */ + +bool cmp_write_pfix(cmp_ctx_t *ctx, uint8_t c); +bool cmp_write_nfix(cmp_ctx_t *ctx, int8_t c); + +bool cmp_write_sfix(cmp_ctx_t *ctx, int8_t c); +bool cmp_write_s8(cmp_ctx_t *ctx, int8_t c); +bool cmp_write_s16(cmp_ctx_t *ctx, int16_t s); +bool cmp_write_s32(cmp_ctx_t *ctx, int32_t i); +bool cmp_write_s64(cmp_ctx_t *ctx, int64_t l); + +bool cmp_write_ufix(cmp_ctx_t *ctx, uint8_t c); +bool cmp_write_u8(cmp_ctx_t *ctx, uint8_t c); +bool cmp_write_u16(cmp_ctx_t *ctx, uint16_t s); +bool cmp_write_u32(cmp_ctx_t *ctx, uint32_t i); +bool cmp_write_u64(cmp_ctx_t *ctx, uint64_t l); + +bool cmp_write_float(cmp_ctx_t *ctx, float f); +bool cmp_write_double(cmp_ctx_t *ctx, double d); + +bool cmp_write_fixstr_marker(cmp_ctx_t *ctx, uint8_t size); +bool cmp_write_fixstr(cmp_ctx_t *ctx, const char *data, uint8_t size); +bool cmp_write_str8_marker(cmp_ctx_t *ctx, uint8_t size); +bool cmp_write_str8(cmp_ctx_t *ctx, const char *data, uint8_t size); +bool cmp_write_str16_marker(cmp_ctx_t *ctx, uint16_t size); +bool cmp_write_str16(cmp_ctx_t *ctx, const char *data, uint16_t size); +bool cmp_write_str32_marker(cmp_ctx_t *ctx, uint32_t size); +bool cmp_write_str32(cmp_ctx_t *ctx, const char *data, uint32_t size); + +bool cmp_write_bin8_marker(cmp_ctx_t *ctx, uint8_t size); +bool cmp_write_bin8(cmp_ctx_t *ctx, const void *data, uint8_t size); +bool cmp_write_bin16_marker(cmp_ctx_t *ctx, uint16_t size); +bool cmp_write_bin16(cmp_ctx_t *ctx, const void *data, uint16_t size); +bool cmp_write_bin32_marker(cmp_ctx_t *ctx, uint32_t size); +bool cmp_write_bin32(cmp_ctx_t *ctx, const void *data, uint32_t size); + +bool cmp_write_fixarray(cmp_ctx_t *ctx, uint8_t size); +bool cmp_write_array16(cmp_ctx_t *ctx, uint16_t size); +bool cmp_write_array32(cmp_ctx_t *ctx, uint32_t size); + +bool cmp_write_fixmap(cmp_ctx_t *ctx, uint8_t size); +bool cmp_write_map16(cmp_ctx_t *ctx, uint16_t size); +bool cmp_write_map32(cmp_ctx_t *ctx, uint32_t size); + +bool cmp_write_fixext1_marker(cmp_ctx_t *ctx, int8_t type); +bool cmp_write_fixext1(cmp_ctx_t *ctx, int8_t type, const void *data); +bool cmp_write_fixext2_marker(cmp_ctx_t *ctx, int8_t type); +bool cmp_write_fixext2(cmp_ctx_t *ctx, int8_t type, const void *data); +bool cmp_write_fixext4_marker(cmp_ctx_t *ctx, int8_t type); +bool cmp_write_fixext4(cmp_ctx_t *ctx, int8_t type, const void *data); +bool cmp_write_fixext8_marker(cmp_ctx_t *ctx, int8_t type); +bool cmp_write_fixext8(cmp_ctx_t *ctx, int8_t type, const void *data); +bool cmp_write_fixext16_marker(cmp_ctx_t *ctx, int8_t type); +bool cmp_write_fixext16(cmp_ctx_t *ctx, int8_t type, const void *data); + +bool cmp_write_ext8_marker(cmp_ctx_t *ctx, int8_t type, uint8_t size); +bool cmp_write_ext8(cmp_ctx_t *ctx, int8_t type, uint8_t size, + const void *data); +bool cmp_write_ext16_marker(cmp_ctx_t *ctx, int8_t type, uint16_t size); +bool cmp_write_ext16(cmp_ctx_t *ctx, int8_t type, uint16_t size, + const void *data); +bool cmp_write_ext32_marker(cmp_ctx_t *ctx, int8_t type, uint32_t size); +bool cmp_write_ext32(cmp_ctx_t *ctx, int8_t type, uint32_t size, + const void *data); + +bool cmp_read_pfix(cmp_ctx_t *ctx, uint8_t *c); +bool cmp_read_nfix(cmp_ctx_t *ctx, int8_t *c); + +bool cmp_read_sfix(cmp_ctx_t *ctx, int8_t *c); +bool cmp_read_s8(cmp_ctx_t *ctx, int8_t *c); +bool cmp_read_s16(cmp_ctx_t *ctx, int16_t *s); +bool cmp_read_s32(cmp_ctx_t *ctx, int32_t *i); +bool cmp_read_s64(cmp_ctx_t *ctx, int64_t *l); + +bool cmp_read_ufix(cmp_ctx_t *ctx, uint8_t *c); +bool cmp_read_u8(cmp_ctx_t *ctx, uint8_t *c); +bool cmp_read_u16(cmp_ctx_t *ctx, uint16_t *s); +bool cmp_read_u32(cmp_ctx_t *ctx, uint32_t *i); +bool cmp_read_u64(cmp_ctx_t *ctx, uint64_t *l); + +bool cmp_read_float(cmp_ctx_t *ctx, float *f); +bool cmp_read_double(cmp_ctx_t *ctx, double *d); + +bool cmp_read_fixext1_marker(cmp_ctx_t *ctx, int8_t *type); +bool cmp_read_fixext1(cmp_ctx_t *ctx, int8_t *type, void *data); +bool cmp_read_fixext2_marker(cmp_ctx_t *ctx, int8_t *type); +bool cmp_read_fixext2(cmp_ctx_t *ctx, int8_t *type, void *data); +bool cmp_read_fixext4_marker(cmp_ctx_t *ctx, int8_t *type); +bool cmp_read_fixext4(cmp_ctx_t *ctx, int8_t *type, void *data); +bool cmp_read_fixext8_marker(cmp_ctx_t *ctx, int8_t *type); +bool cmp_read_fixext8(cmp_ctx_t *ctx, int8_t *type, void *data); +bool cmp_read_fixext16_marker(cmp_ctx_t *ctx, int8_t *type); +bool cmp_read_fixext16(cmp_ctx_t *ctx, int8_t *type, void *data); + +bool cmp_read_ext8_marker(cmp_ctx_t *ctx, int8_t *type, uint8_t *size); +bool cmp_read_ext8(cmp_ctx_t *ctx, int8_t *type, uint8_t *size, void *data); +bool cmp_read_ext16_marker(cmp_ctx_t *ctx, int8_t *type, uint16_t *size); +bool cmp_read_ext16(cmp_ctx_t *ctx, int8_t *type, uint16_t *size, void *data); +bool cmp_read_ext32_marker(cmp_ctx_t *ctx, int8_t *type, uint32_t *size); +bool cmp_read_ext32(cmp_ctx_t *ctx, int8_t *type, uint32_t *size, void *data); + +/* + * ============================================================================ + * === Object API + * ============================================================================ + */ + +bool cmp_object_is_char(cmp_object_t *obj); +bool cmp_object_is_short(cmp_object_t *obj); +bool cmp_object_is_int(cmp_object_t *obj); +bool cmp_object_is_long(cmp_object_t *obj); +bool cmp_object_is_sinteger(cmp_object_t *obj); +bool cmp_object_is_uchar(cmp_object_t *obj); +bool cmp_object_is_ushort(cmp_object_t *obj); +bool cmp_object_is_uint(cmp_object_t *obj); +bool cmp_object_is_ulong(cmp_object_t *obj); +bool cmp_object_is_uinteger(cmp_object_t *obj); +bool cmp_object_is_float(cmp_object_t *obj); +bool cmp_object_is_double(cmp_object_t *obj); +bool cmp_object_is_nil(cmp_object_t *obj); +bool cmp_object_is_bool(cmp_object_t *obj); +bool cmp_object_is_str(cmp_object_t *obj); +bool cmp_object_is_bin(cmp_object_t *obj); +bool cmp_object_is_array(cmp_object_t *obj); +bool cmp_object_is_map(cmp_object_t *obj); +bool cmp_object_is_ext(cmp_object_t *obj); + +bool cmp_object_as_char(cmp_object_t *obj, int8_t *c); +bool cmp_object_as_short(cmp_object_t *obj, int16_t *s); +bool cmp_object_as_int(cmp_object_t *obj, int32_t *i); +bool cmp_object_as_long(cmp_object_t *obj, int64_t *d); +bool cmp_object_as_sinteger(cmp_object_t *obj, int64_t *d); +bool cmp_object_as_uchar(cmp_object_t *obj, uint8_t *c); +bool cmp_object_as_ushort(cmp_object_t *obj, uint16_t *s); +bool cmp_object_as_uint(cmp_object_t *obj, uint32_t *i); +bool cmp_object_as_ulong(cmp_object_t *obj, uint64_t *u); +bool cmp_object_as_uinteger(cmp_object_t *obj, uint64_t *u); +bool cmp_object_as_float(cmp_object_t *obj, float *f); +bool cmp_object_as_double(cmp_object_t *obj, double *d); +bool cmp_object_as_bool(cmp_object_t *obj, bool *b); +bool cmp_object_as_str(cmp_object_t *obj, uint32_t *size); +bool cmp_object_as_bin(cmp_object_t *obj, uint32_t *size); +bool cmp_object_as_array(cmp_object_t *obj, uint32_t *size); +bool cmp_object_as_map(cmp_object_t *obj, uint32_t *size); +bool cmp_object_as_ext(cmp_object_t *obj, int8_t *type, uint32_t *size); + +bool cmp_object_to_str(cmp_ctx_t *ctx, cmp_object_t *obj, char *data, uint32_t buf_size); +bool cmp_object_to_bin(cmp_ctx_t *ctx, cmp_object_t *obj, void *data, uint32_t buf_size); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +/* + * ============================================================================ + * === Backwards compatibility defines + * ============================================================================ + */ + +#define cmp_write_int cmp_write_integer +#define cmp_write_sint cmp_write_integer +#define cmp_write_sinteger cmp_write_integer +#define cmp_write_uint cmp_write_uinteger +#define cmp_read_sinteger cmp_read_integer + +#endif /* CMP_H__ */ + +/* vi: set et ts=2 sw=2: */ + diff --git a/gateway.c b/gateway.c index d11e7df..d62ed9e 100644 --- a/gateway.c +++ b/gateway.c @@ -36,6 +36,7 @@ #include "config.h" #include "gui.h" #include "listener.h" +#include "habpack.h" #include "udpclient.h" #define VERSION "V1.8.19" @@ -173,7 +174,7 @@ int help_win_displayed = 0; pthread_mutex_t var = PTHREAD_MUTEX_INITIALIZER; -#pragma pack(1) +#pragma pack(push,1) struct TBinaryPacket { uint8_t PayloadIDs; @@ -184,6 +185,8 @@ struct TBinaryPacket { uint16_t Altitude; }; +#pragma pack(pop) + // Create pipes for inter proces communication // GLOBAL AS CALLED FROM INTERRRUPT int telem_pipe_fd[2]; @@ -746,6 +749,54 @@ void ProcessCallingMessage(int Channel, char *Message) } } +void ProcessCallingHABpack(int Channel, received_t *Received) +{ + double Frequency; + + ChannelPrintf( Channel, 3, 1, "Calling message (HABpack)"); + + Frequency = (double)Received->Telemetry.DownlinkFrequency / 1000000; + + if (Config.LoRaDevices[Channel].AFC) + { + Frequency += (Config.LoRaDevices[Channel].activeFreq - Config.LoRaDevices[Channel].Frequency); + } + + // Decoded OK + setMode( Channel, RF98_MODE_SLEEP ); + + setFrequency( Channel, Frequency ); + + if(Received->Telemetry.DownlinkLoraMode >= 0) + { + + SetLoRaParameters(Channel, + LoRaModes[Received->Telemetry.DownlinkLoraMode].ImplicitOrExplicit, + ECToInt(LoRaModes[Received->Telemetry.DownlinkLoraMode].ErrorCoding), + BandwidthToDouble(LoRaModes[Received->Telemetry.DownlinkLoraMode].Bandwidth), + SFToInt(LoRaModes[Received->Telemetry.DownlinkLoraMode].SpreadingFactor), + LowOptToInt(LoRaModes[Received->Telemetry.DownlinkLoraMode].LowDataRateOptimize) + ); + } + else + { + SetLoRaParameters(Channel, + Received->Telemetry.DownlinkLoraImplicit, + Received->Telemetry.DownlinkLoraErrorCoding, + Received->Telemetry.DownlinkLoraBandwidth, + Received->Telemetry.DownlinkLoraSpreadingFactor, + Received->Telemetry.DownlinkLoraLowDatarateOptimise + ); + } + + setMode( Channel, RF98_MODE_RX_CONTINUOUS ); + + LogMessage( "Ch %d: Calling message, new frequency %7.3lf\n", Channel, + Frequency ); + + Config.LoRaDevices[Channel].InCallingMode = 1; +} + void RemoveOldPayloads(void) { int i; @@ -831,23 +882,23 @@ void DoPositionCalcs(int PayloadIndex) Config.Payloads[PayloadIndex].PreviousAltitude = Config.Payloads[PayloadIndex].Altitude; } -void ProcessLine(int Channel, char *Line) +void ProcessLineUKHAS(int Channel, char *Line) { int PayloadIndex; - char Payload[32], OziSentence[200]; + char Payload[32]; - // Find free position for this payload - sscanf(Line + 2, "%31[^,]", Payload); - PayloadIndex = FindFreePayload(Payload); + // Find free position for this payload + sscanf(Line + 2, "%31[^,]", Payload); + PayloadIndex = FindFreePayload(Payload); - // Store sentence against this payload + // Store sentence against this payload strcpy(Config.Payloads[PayloadIndex].Telemetry, Line); // Fill in source channel Config.Payloads[PayloadIndex].Channel = Channel; // Parse key fields from sentence - sscanf( Line + 2, "%15[^,],%u,%8[^,],%lf,%lf,%u", + sscanf( Line + 2, "%15[^,],%u,%8[^,],%lf,%lf,%d", (Config.Payloads[PayloadIndex].Payload), &(Config.Payloads[PayloadIndex].Counter), (Config.Payloads[PayloadIndex].Time), @@ -863,41 +914,89 @@ void ProcessLine(int Channel, char *Line) // Ascent rate DoPositionCalcs(PayloadIndex); - - // Update display - ChannelPrintf(Channel, 4, 1, "%8.5lf, %8.5lf, %05u ", + + // Update display + ChannelPrintf(Channel, 4, 1, "%8.5lf, %8.5lf, %05d ", Config.Payloads[PayloadIndex].Latitude, Config.Payloads[PayloadIndex].Longitude, - Config.Payloads[PayloadIndex].Altitude); - - // Send out to any OziMux clients - if (Config.OziPort > 0) - { - sprintf(OziSentence, "TELEMETRY,%s,%lf,%lf,%u\n", - Config.Payloads[PayloadIndex].Time, - Config.Payloads[PayloadIndex].Latitude, - Config.Payloads[PayloadIndex].Longitude, - Config.Payloads[PayloadIndex].Altitude); - UDPSend(OziSentence, Config.OziPort); - } + Config.Payloads[PayloadIndex].Altitude); + + // Send out to any OziMux clients + if (Config.OziPort > 0) + { + char OziSentence[200]; + sprintf(OziSentence, "TELEMETRY,%s,%lf,%lf,%d\n", + Config.Payloads[PayloadIndex].Time, + Config.Payloads[PayloadIndex].Latitude, + Config.Payloads[PayloadIndex].Longitude, + Config.Payloads[PayloadIndex].Altitude); + UDPSend(OziSentence, Config.OziPort); + } +} + +void ProcessLineHABpack(int Channel, received_t *Received) +{ + int PayloadIndex; + + PayloadIndex = FindFreePayload(Received->Telemetry.Callsign); + + // Store sentence against this payload + strcpy(Config.Payloads[PayloadIndex].Telemetry, Received->UKHASstring); + + // Fill in source channel + Config.Payloads[PayloadIndex].Channel = Channel; + + strncpy(Config.Payloads[PayloadIndex].Payload, Received->Telemetry.Callsign, 15); + Config.Payloads[PayloadIndex].Counter = Received->Telemetry.SentenceId; + strncpy(Config.Payloads[PayloadIndex].Time, Received->Telemetry.TimeString, 8); + Config.Payloads[PayloadIndex].Latitude = Received->Telemetry.Latitude; + Config.Payloads[PayloadIndex].Longitude = Received->Telemetry.Longitude; + Config.Payloads[PayloadIndex].Altitude = Received->Telemetry.Altitude; + + // Mark when this was received, so we can time-out old payloads + Config.Payloads[PayloadIndex].LastPacketAt = time(NULL); + + // Mark for server socket to send to client + Config.Payloads[PayloadIndex].SendToClients = 1; + + // Ascent rate + DoPositionCalcs(PayloadIndex); + + // Update display + ChannelPrintf(Channel, 4, 1, "%8.5lf, %8.5lf, %05d ", + Config.Payloads[PayloadIndex].Latitude, + Config.Payloads[PayloadIndex].Longitude, + Config.Payloads[PayloadIndex].Altitude); + + // Send out to any OziMux clients + if (Config.OziPort > 0) + { + char OziSentence[200]; + sprintf(OziSentence, "TELEMETRY,%s,%lf,%lf,%d\n", + Config.Payloads[PayloadIndex].Time, + Config.Payloads[PayloadIndex].Latitude, + Config.Payloads[PayloadIndex].Longitude, + Config.Payloads[PayloadIndex].Altitude); + UDPSend(OziSentence, Config.OziPort); + } } -void ProcessTelemetryMessage(int Channel, char *Message, rx_metadata_t *Metadata) +void ProcessTelemetryMessage(int Channel, received_t *Received) { - if (strlen(Message + 1) < 250) + if (strlen(Received->UKHASstring) < 250) { char *startmessage, *endmessage; char telem[40]; char buffer[40]; - sprintf(telem, "Telemetry %d bytes", strlen( Message + 1 )); + sprintf(telem, "Telemetry %d bytes", strlen( Received->UKHASstring )); // Pad the string with spaces to the size of the window sprintf(buffer,"%-37s", telem ); ChannelPrintf( Channel, 3, 1, buffer); - startmessage = Message + strspn(Message, "$%") - 2; + startmessage = Received->UKHASstring + strspn(Received->UKHASstring, "$%") - 2; endmessage = strchr( startmessage, '\n' ); if (endmessage == NULL) endmessage = strchr(startmessage, 0); @@ -916,22 +1015,12 @@ void ProcessTelemetryMessage(int Channel, char *Message, rx_metadata_t *Metadata } strcpy( Config.LoRaDevices[Channel].Telemetry, startmessage ); - - ProcessLine(Channel, startmessage); - - tm = localtime( &Metadata->Timestamp ); - + Config.LoRaDevices[Channel].TelemetryCount++; if ( Config.EnableHabitat ) { - - // Create a telemetry packet - telemetry_t t; - memcpy( t.Telemetry, startmessage, strlen( startmessage ) + 1 ); - memcpy( &t.Metadata, Metadata, sizeof( rx_metadata_t ) ); - // Add the telemetry packet to the pipe - int result = write( telem_pipe_fd[1], &t, sizeof( t ) ); + int result = write( telem_pipe_fd[1], Received, sizeof( *Received ) ); if ( result == -1 ) { exit_error("Error writing to the telemetry pipe\n"); @@ -945,16 +1034,15 @@ void ProcessTelemetryMessage(int Channel, char *Message, rx_metadata_t *Metadata htsv.packet_count++; } } - - Config.LoRaDevices[Channel].TelemetryCount++; + tm = localtime( &Received->Metadata.Timestamp ); LogMessage("%02d:%02d:%02d Ch%d: %s%s\n", tm->tm_hour, tm->tm_min, tm->tm_sec, Channel, startmessage, Repeated ? " (repeated)" : ""); startmessage = endmessage + 1; endmessage = strchr( startmessage, '\n' ); } - Config.LoRaDevices[Channel].LastTelemetryPacketAt = Metadata->Timestamp; + Config.LoRaDevices[Channel].LastTelemetryPacketAt = Received->Metadata.Timestamp; } } @@ -1015,9 +1103,10 @@ FileExists( char *filename ) return stat( filename, &st ) == 0; } -void ProcessSSDVMessage( int Channel, char *Message, int Repeated) +void ProcessSSDVMessage( int Channel, char *Message_in, int Repeated) { // SSDV packet + char Message[257]; uint32_t CallsignCode; char Callsign[7], *FileMode; int ImageNumber, PacketNumber; @@ -1029,6 +1118,9 @@ void ProcessSSDVMessage( int Channel, char *Message, int Repeated) now = time( 0 ); tm = localtime( &now ); + // Move into new buffer with preceding char + memcpy((Message + 1), Message_in, 256); + Message[0] = 0x55; CallsignCode = Message[2]; @@ -1220,17 +1312,14 @@ void DIO0_Interrupt( int Channel ) } else { - int Bytes; - char Message[257]; - rx_metadata_t Metadata; + received_t Received = { .Telemetry = { .habpack_extra = NULL } }; - Metadata.Channel = Channel; - - Bytes = receiveMessage( Channel, Message + 1, &Metadata ); + Received.Metadata.Channel = Channel; + Received.Bytes = receiveMessage( Channel, Received.Message, &Received.Metadata ); Config.LoRaDevices[Channel].GotReply = 1; - if ( Bytes > 0 ) + if ( Received.Bytes > 0 ) { if ( Config.LoRaDevices[Channel].ActivityLED >= 0 ) { @@ -1238,60 +1327,63 @@ void DIO0_Interrupt( int Channel ) LEDCounts[Channel] = 5; } - if ( Message[1] == '!' ) + if ( Received.Message[0] == '!' ) { - ProcessUploadMessage( Channel, Message + 1 ); + ProcessUploadMessage( Channel, Received.Message); } - else if ( Message[1] == '^' ) + else if ( Received.Message[0] == '^' ) { - ProcessCallingMessage( Channel, Message + 1 ); + ProcessCallingMessage( Channel, Received.Message); } - else if ((Message[1] == '$') || (Message[1] == '%')) + else if ((Received.Message[0] == '$') || (Received.Message[0] == '%')) { - ProcessTelemetryMessage(Channel, Message + 1, &Metadata); - TestMessageForSMSAcknowledgement( Channel, Message + 1); - strcpy(Config.LoRaDevices[Channel].LocalDataBuffer, Message+1); + /* RTTY */ + strncpy(Received.UKHASstring, Received.Message, Received.Bytes); + ProcessTelemetryMessage(Channel, &Received); + ProcessLineUKHAS(Channel, Config.LoRaDevices[Channel].Telemetry); + TestMessageForSMSAcknowledgement( Channel, Received.UKHASstring); + strcpy(Config.LoRaDevices[Channel].LocalDataBuffer, Received.UKHASstring); strcat(Config.LoRaDevices[Channel].LocalDataBuffer, "\r\n"); - Config.LoRaDevices[Channel].LocalDataCount = Bytes+1; - UDPSend(Message + 1, Config.UDPPort); + Config.LoRaDevices[Channel].LocalDataCount = Received.Bytes; + UDPSend(Received.UKHASstring, Config.UDPPort); } - else if ( Message[1] == '>' ) + else if ( Received.Message[0] == '>' ) { - LogMessage( "Flight Controller message %d bytes = %s\n", Bytes, Message + 1 ); + LogMessage( "Flight Controller message %d bytes = %s\n", Received.Bytes, Received.Message ); } - else if ( Message[1] == '<' ) + else if ( Received.Message[0] == '<' ) { - LogMessage("Local Data %d bytes = %s", Bytes, Message+1); - strcpy(Config.LoRaDevices[Channel].LocalDataBuffer, Message+1); + LogMessage("Local Data %d bytes = %s", Received.Bytes, Received.Message); + strcpy(Config.LoRaDevices[Channel].LocalDataBuffer, Received.Message); strcat(Config.LoRaDevices[Channel].LocalDataBuffer, "\r\n"); - Config.LoRaDevices[Channel].LocalDataCount = Bytes+1; + Config.LoRaDevices[Channel].LocalDataCount = Received.Bytes; } - else if ( Message[1] == '*' ) + else if ( Received.Message[0] == '*' ) { - LogMessage( "Uplink Command message %d bytes = %s\n", Bytes, Message + 1 ); + LogMessage( "Uplink Command message %d bytes = %s\n", Received.Bytes, Received.Message ); } - else if (Message[1] == '+') + else if ( Received.Message[0] == '+') { - LogMessage( "Telnet Uplink message %d packet ID %d bytes = '%s'\n", Bytes, Message[2], Message + 3); + LogMessage( "Telnet Uplink message %d packet ID %d bytes = '%s'\n", Received.Bytes, Received.Message[1], Received.Message + 2); } - else if ( Message[1] == '-' ) + else if ( Received.Message[0] == '-' ) { - ProcessTelnetMessage(Channel, Message+1, Bytes); + ProcessTelnetMessage(Channel, Received.Message, Received.Bytes); } - else if (((Message[1] & 0x7F) == 0x66) || // SSDV JPG format - ((Message[1] & 0x7F) == 0x67) || // SSDV other formats - ((Message[1] & 0x7F) == 0x68) || - ((Message[1] & 0x7F) == 0x69)) + else if (((Received.Message[0] & 0x7F) == 0x66) || // SSDV JPG format + ((Received.Message[0] & 0x7F) == 0x67) || // SSDV other formats + ((Received.Message[0] & 0x7F) == 0x68) || + ((Received.Message[0] & 0x7F) == 0x69)) { int Repeated; // Handle repeater bit - Repeated = Message[1] & 0x80; - Message[1] &= 0x7F; + Repeated = Received.Message[0] & 0x80; + Received.Message[0] &= 0x7F; - ProcessSSDVMessage( Channel, Message, Repeated); + ProcessSSDVMessage( Channel, Received.Message, Repeated); } - else if ( Message[1] == 0x00) + else if ( Received.Message[0] == 0x00) { time_t now; struct tm *tm; @@ -1301,10 +1393,25 @@ void DIO0_Interrupt( int Channel ) LogMessage("%02d:%02d:%02d Ch%d: Null uplink packet\n", tm->tm_hour, tm->tm_min, tm->tm_sec, Channel); } + else if ( ((Received.Message[0] & 0xF0) == 0x80) || (Received.Message[0] == 0xde)) + { + /* HABpack Binary Message - https://ukhas.org.uk/communication:habpack */ + Habpack_Process_Message(&Received); + if(Received.isCallingBeacon == false) + { + ProcessTelemetryMessage(Channel, &Received); + ProcessLineHABpack(Channel, &Received); + TestMessageForSMSAcknowledgement(Channel, Received.UKHASstring); + } + else + { + ProcessCallingHABpack(Channel, &Received); + } + } else { - LogMessage("Unknown packet type is %02Xh, RSSI %d\n", Message[1], PacketRSSI(Channel)); - ChannelPrintf( Channel, 3, 1, "Unknown Packet %d, %d bytes", Message[0], Bytes); + LogMessage("Unknown packet type is %02Xh, RSSI %d\n", Received.Message[0], PacketRSSI(Channel)); + ChannelPrintf( Channel, 3, 1, "Unknown Packet %d, %d bytes", Received.Message[0], Received.Bytes); Config.LoRaDevices[Channel].UnknownCount++; } @@ -1322,6 +1429,8 @@ void DIO0_Interrupt( int Channel ) ShowPacketCounts( Channel ); } + /* Free habpack linked list */ + Habpack_Telem_Destroy(&Received); } } @@ -1783,7 +1892,7 @@ WINDOW *InitDisplay(void) char buffer[80]; - sprintf( buffer, "LoRa Habitat and SSDV Gateway by M0RPI, M0RJX - " VERSION); + sprintf( buffer, "LoRa Habitat and SSDV Gateway by M0RPI, M0DNY, M0RJX - " VERSION); // Title bar mvaddstr( 0, ( 80 - strlen( buffer ) ) / 2, buffer ); @@ -2496,5 +2605,4 @@ int main( int argc, char **argv ) return 0; -} - +} \ No newline at end of file diff --git a/global.h b/global.h index ef7cf1b..1efc595 100644 --- a/global.h +++ b/global.h @@ -1,39 +1,46 @@ #ifndef _H_Global #define _H_Global -#include - +#include + #define RUNNING 1 // The main program is running #define STOPPED 0 // The main program has stopped - -#define MAX_PAYLOADS 32 - -struct TPayload -{ + +#define MAX_PAYLOADS 32 + +struct TPayload +{ int InUse; - int SendToClients; - int Channel; - - time_t LastPacketAt; - - char Telemetry[256]; - char Payload[32]; - - char Time[12]; - unsigned int Counter, LastCounter; - double Longitude, Latitude; - unsigned int Altitude, PreviousAltitude; - float AscentRate; - unsigned long LastPositionAt; -}; struct TLoRaDevice { - double Frequency; - double Bandwidth; - double CurrentBandwidth; - int InUse; int DIO0; int DIO5; double activeFreq; + int SendToClients; + int Channel; - int AFC; // Enable Automatic Frequency Control - double MaxAFCStep; // Maximum adjustment, in kHz, per packet - int AFCTimeout; // Revert to original frequency if no packets for this period (in seconds) + time_t LastPacketAt; + + char Telemetry[256]; + char Payload[32]; + + char Time[12]; + unsigned int Counter, LastCounter; + double Longitude, Latitude; + int Altitude, PreviousAltitude; + float AscentRate; + unsigned long LastPositionAt; +}; + +struct TLoRaDevice +{ + double Frequency; + double Bandwidth; + double CurrentBandwidth; + + int InUse; + int DIO0; + int DIO5; + double activeFreq; + + int AFC; // Enable Automatic Frequency Control + double MaxAFCStep; // Maximum adjustment, in kHz, per packet + int AFCTimeout; // Revert to original frequency if no packets for this period (in seconds) int SpeedMode; int Power; @@ -50,33 +57,33 @@ struct TPayload // unsigned int Satellites; time_t LastSSDVPacketAt, LastTelemetryPacketAt; time_t ReturnToCallingModeAt; - time_t ReturnToOriginalFrequencyAt; + time_t ReturnToOriginalFrequencyAt; int InCallingMode; - int ActivityLED; - double UplinkFrequency; + int ActivityLED; + double UplinkFrequency; int UplinkMode; - - // Normal (non TDM) uplink + + // Normal (non TDM) uplink int UplinkTime; - int UplinkCycle; - int IdleUplink; - int SSDVUplink; - char UplinkMessage[256]; - - // Telnet uplink - char Telemetry[256]; - unsigned char PacketID; - int TimeSinceLastTx; - int HABAck; // Received message ACKd our last Tx - int HABConnected; // 1 if HAB has telnet connection - char ToTelnetBuffer[256]; // Buffer for data Telnet --> LoRa - int ToTelnetBufferCount; - char FromTelnetBuffer[256]; // Buffer for data LoRa --> Telnet - int FromTelnetBufferCount; - char HABUplink[256]; - int HABUplinkCount; - int GotHABReply; - int GotReply; + int UplinkCycle; + int IdleUplink; + int SSDVUplink; + char UplinkMessage[256]; + + // Telnet uplink + char Telemetry[256]; + unsigned char PacketID; + int TimeSinceLastTx; + int HABAck; // Received message ACKd our last Tx + int HABConnected; // 1 if HAB has telnet connection + char ToTelnetBuffer[256]; // Buffer for data Telnet --> LoRa + int ToTelnetBufferCount; + char FromTelnetBuffer[256]; // Buffer for data LoRa --> Telnet + int FromTelnetBufferCount; + char HABUplink[256]; + int HABUplinkCount; + int GotHABReply; + int GotReply; // Local data packets int LocalDataCount; @@ -87,9 +94,13 @@ struct TPayload int PacketSNR, PacketRSSI; double FrequencyError; }; - struct TConfig { char Tracker[16]; // Callsign or name of receiver - double latitude, longitude; // Receiver's location - int EnableHabitat; + +struct TConfig + { + char Tracker[16]; // Callsign or name of receiver + double latitude, longitude; // Receiver's location + + int EnableHabitat; int EnableSSDV; int EnableTelemetryLogging; int EnablePacketLogging; @@ -99,23 +110,24 @@ struct TPayload char ftpUser[32]; char ftpPassword[32]; char ftpFolder[64]; - struct TLoRaDevice LoRaDevices[2]; + struct TLoRaDevice LoRaDevices[2]; struct TPayload Payloads[MAX_PAYLOADS]; int NetworkLED; int InternetLED; int ServerPort; // JSON port for telemetry, settings int UDPPort; // UDP Broadcast port for raw received data packets - int OziPort; // UDP Broadcast port for OziMux formatted packets - int HABPort; // Telnet style port for comms with HAB - int HABTimeout; // Timeout in ms for telnet uplink - int HABChannel; // LoRa Channel for uplink + int OziPort; // UDP Broadcast port for OziMux formatted packets + int HABPort; // Telnet style port for comms with HAB + int HABTimeout; // Timeout in ms for telnet uplink + int HABChannel; // LoRa Channel for uplink int DataPort; // Raw received data packet port char SMSFolder[64]; char antenna[64]; - int EnableDev; - char UplinkCode[64]; + int EnableDev; + char UplinkCode[64]; }; - typedef struct { + +typedef struct { int parent_status; unsigned long packet_count; } thread_shared_vars_t; @@ -135,28 +147,72 @@ typedef struct { int RSSI; } rx_metadata_t; +/* HABpack Telemetry Storage */ +typedef enum { HB_VALUE_INTEGER, HB_VALUE_REAL } HB_Value_Type; +typedef struct habpack_telem_linklist_entry { + uint32_t type; + char name[32]; + HB_Value_Type value_type; + union { + int64_t integer; + double real; + } value; + struct habpack_telem_linklist_entry *next; +} habpack_telem_linklist_entry_t; + typedef struct { - char Telemetry[257]; + /* Core Telemetry */ + char Callsign[32]; + uint64_t SentenceId; + uint64_t Time; + char TimeString[9]; + double Latitude; + double Longitude; + int64_t Altitude; + /* Calling Beacon Values */ + uint64_t DownlinkFrequency; + int64_t DownlinkLoraMode; + int DownlinkLoraImplicit; + int DownlinkLoraErrorCoding; + double DownlinkLoraBandwidth; + int DownlinkLoraSpreadingFactor; + int DownlinkLoraLowDatarateOptimise; + /* Linked list of additional telem fields */ + habpack_telem_linklist_entry_t *habpack_extra; +} rx_telemetry_t; + +#define Message_length 257 +#define UKHASstring_length 257 +typedef struct { + char Message[Message_length]; + int Bytes; + /* UKHAS ASCII Telemetry String for habitat upload */ + char UKHASstring[UKHASstring_length]; + bool isCallingBeacon; + double AscentRate; + /* Telemetry values */ + rx_telemetry_t Telemetry; rx_metadata_t Metadata; -} telemetry_t; +} received_t; typedef struct { short int Channel; char SSDV_Packet[257]; int Packet_Number; } ssdv_t; - -struct TServerInfo -{ - int Port; - int Connected; - int ServerIndex; - int sockfd; -}; + +struct TServerInfo +{ + int Port; + int Connected; + int ServerIndex; + int sockfd; +}; extern struct TConfig Config; - extern int SSDVSendArrayIndex; - extern pthread_mutex_t ssdv_mutex; - void LogMessage( const char *format, ... ); +extern int SSDVSendArrayIndex; +extern pthread_mutex_t ssdv_mutex; + +void LogMessage( const char *format, ... ); #endif /* _H_Global */ diff --git a/habitat.c b/habitat.c index e0c7a54..c519499 100644 --- a/habitat.c +++ b/habitat.c @@ -46,7 +46,7 @@ void hash_to_hex( unsigned char *hash, char *line ) line[64] = '\0'; } -void UploadTelemetryPacket( telemetry_t * t ) +void UploadTelemetryPacket( received_t * t ) { CURL *curl; CURLcode res; @@ -93,7 +93,7 @@ void UploadTelemetryPacket( telemetry_t * t ) curl_easy_setopt( curl, CURLOPT_NOSIGNAL, 1 ); // Grab current telemetry string and append a linefeed - sprintf( Sentence, "%s\n", t->Telemetry ); + sprintf( Sentence, "%s\n", t->UKHASstring ); // Convert sentence to base64 base64_encode( Sentence, strlen( Sentence ), &base64_length, @@ -174,7 +174,7 @@ void *HabitatLoop( void *vars ) { thread_shared_vars_t *htsv; htsv = vars; - telemetry_t t; + received_t t; int packets = 0; unsigned long total_packets = 0; diff --git a/habpack.c b/habpack.c new file mode 100644 index 0000000..1daa047 --- /dev/null +++ b/habpack.c @@ -0,0 +1,1217 @@ +#include +#include +#include +#include +#include + +#include "habpack.h" +#include "global.h" +#include "base64.h" + +#include "cmp.h" + +extern void LogMessage( const char *format, ... ); + +#define SECONDS_IN_A_DAY 86400 + +static int hb_buf_ptr = 0; +static int hb_ptr_max = 20; + +static bool file_reader(cmp_ctx_t *ctx, void *data, size_t limit) +{ + uint32_t i; + + if (hb_buf_ptr + limit >= hb_ptr_max) + return false; + + for(i = 0; i < limit; i++) + { + *(uint8_t *)data = ((uint8_t *)(ctx->buf))[hb_buf_ptr]; + data++; + hb_buf_ptr++; + } + + return true; +} + +static bool file_skipper(struct cmp_ctx_s *ctx, size_t count) +{ + if (hb_buf_ptr + count >= hb_ptr_max) + return false; + + hb_buf_ptr += count; + + return true; +} + +static size_t file_writer(cmp_ctx_t *ctx, const void *data, size_t count) +{ + uint32_t i; + + if (hb_buf_ptr + count >= hb_ptr_max) + return 0; + + for (i = 0; i < count; i++) + { + ((char*)ctx->buf)[hb_buf_ptr] = *((uint8_t*)data); + data++; + hb_buf_ptr++; + } + + return count; +} + +static uint16_t CRC16(unsigned char *ptr) +{ + uint16_t CRC; + int j; + + CRC = 0xffff; + + for ( ; *ptr; ptr++ ) + { + CRC ^= ( ( ( unsigned int ) *ptr ) << 8 ); + for ( j = 0; j < 8; j++ ) + { + if ( CRC & 0x8000 ) + CRC = ( CRC << 1 ) ^ 0x1021; + else + CRC <<= 1; + } + } + + return CRC; +} + +#define TRY_PARSE_STRING(processing) \ + if(cmp_object_is_str(&item) \ + && cmp_object_to_str(&cmp, &item, &temp[temp_ptr], (uint32_t)(temp_remaining))) \ + { \ + processing \ + } + +#define TRY_PARSE_FLOAT(processing) \ + if(cmp_object_is_float(&item) \ + && cmp_object_as_float(&item, &tf32)) \ + { \ + processing \ + } + +#define TRY_PARSE_DOUBLE(processing) \ + if(cmp_object_is_double(&item) \ + && cmp_object_as_double(&item, &tf64)) \ + { \ + processing \ + } + +#define TRY_PARSE_UNSIGNED_INTEGER(processing) \ + if(cmp_object_is_uinteger(&item) \ + && cmp_object_as_uinteger(&item, &tu64)) \ + { \ + processing \ + } + +#define TRY_PARSE_SIGNED_INTEGER(processing) \ + if(cmp_object_is_sinteger(&item) \ + && cmp_object_as_sinteger(&item, &ts64)) \ + { \ + processing \ + } + +#define TRY_PARSE_ARRAY(processing) \ + if(cmp_object_is_array(&item) \ + && cmp_object_as_array(&item, &array_size)) \ + { \ + for (j = 0; j < array_size; j++) \ + { \ + /* Read array element */ \ + if(!cmp_read_object(&cmp, &item)) \ + return 0; \ + \ + processing \ + } \ + } + +#define TRY_PARSE_ARRAY_AT_ONCE(processing) \ + if(cmp_object_is_array(&item) \ + && cmp_object_as_array(&item, &array_size)) \ + { \ + processing \ + } + +#define TRY_PARSE_ARRAY_AT_ONCE_NESTED(processing) \ + if(cmp_object_is_array(&item) \ + && cmp_object_as_array(&item, &array_size2)) \ + { \ + processing \ + } + +void Habpack_telem_store_field(habpack_telem_linklist_entry_t *initial_entry, uint64_t map_id, char *field_name, int64_t *ptr_integer, double *ptr_real) +{ + habpack_telem_linklist_entry_t *walk_ptr; + + walk_ptr = initial_entry; + while(walk_ptr != NULL) + { + walk_ptr = walk_ptr->next; + } + + walk_ptr = malloc(sizeof(habpack_telem_linklist_entry_t)); + + walk_ptr->type = map_id; + strncpy(walk_ptr->name, field_name, 31); + if(ptr_integer != NULL) + { + walk_ptr->value_type = HB_VALUE_INTEGER; + walk_ptr->value.integer = *ptr_integer; + } + else if(ptr_real != NULL) + { + walk_ptr->value_type = HB_VALUE_REAL; + walk_ptr->value.real = *ptr_real; + } + walk_ptr->next = NULL; +} + + +#define HABPACK_TELEM_STORE_AS_INT64(value) \ + ts64 = (int64_t)value; \ + Habpack_telem_store_field( \ + Received->Telemetry.habpack_extra, \ + map_id, \ + field_name, \ + &ts64, \ + NULL \ + ) + +#define HABPACK_TELEM_STORE_AS_DOUBLE(value) \ + tf64 = (double)value; \ + Habpack_telem_store_field( \ + Received->Telemetry.habpack_extra, \ + map_id, \ + field_name, \ + NULL, \ + &tf64 \ + ) + +void Habpack_Telem_Destroy(received_t *Received) +{ + habpack_telem_linklist_entry_t *walk_ptr, *last_ptr; + + walk_ptr = Received->Telemetry.habpack_extra; + while(walk_ptr != NULL) + { + last_ptr = walk_ptr; + walk_ptr = walk_ptr->next; + + free(last_ptr); + } +} + + +int Habpack_Process_Message(received_t *Received) +{ + cmp_ctx_t cmp; + uint32_t map_size; + uint32_t array_size,array_size2; + uint32_t i,j,k; + + uint64_t map_id; + uint64_t tu64; + int64_t ts64; + float tf32; + double tf64; + char field_name[32]; + + //When converting habpack to $$uhkas, the habpack fields should be ordered. + //This stores the output prior to reordering + char temp[1024]; + memset(temp,0,sizeof(char)*1024); + int temp_ptr = 0; + int temp_remaining = 1024-temp_ptr; + int out_ptr = 0; + + uint8_t keys[128]; + uint16_t key_val[128]; + uint8_t key_ptr = 0; + cmp_object_t item; + + cmp_init(&cmp, (void*)Received->Message, file_reader, file_skipper, file_writer); + + hb_buf_ptr = 0; + hb_ptr_max = 256; + + Received->isCallingBeacon = false; + + /* Open MessagePack Map */ + if (!cmp_read_map(&cmp, &map_size)) + return 0; + + /* Loop through each element of the Map */ + for (i=0; i < map_size; i++) + { + /* Read Map ID (aka HABpack field ID) */ + if (!(cmp_read_uinteger(&cmp, &map_id))) + return 0; + + /* Read HABpack field object */ + if(!cmp_read_object(&cmp, &item)) + return 0; + + /* Parse according to Map ID (aka HABpack field ID) */ + switch(map_id) + { + case HABPACK_CALLSIGN: + key_val[key_ptr] = temp_ptr; + keys[key_ptr++] = map_id; + + temp_remaining = 1024-temp_ptr; + + TRY_PARSE_STRING( + strncpy(Received->Telemetry.Callsign, &temp[temp_ptr], 31); + + while(temp[temp_ptr]) + temp_ptr++; + ) + else TRY_PARSE_UNSIGNED_INTEGER( + snprintf(Received->Telemetry.Callsign, 31, "%lld", tu64); + + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%lld",tu64); + ) + else return 0; + + temp_ptr++; + break; + + case HABPACK_SENTENCE_ID: + key_val[key_ptr] = temp_ptr; + keys[key_ptr++] = map_id; + + TRY_PARSE_UNSIGNED_INTEGER( + Received->Telemetry.SentenceId = tu64; + + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%lld",tu64); + ) + else return 0; + + temp_ptr++; + break; + + case HABPACK_TIME: + key_val[key_ptr] = temp_ptr; + keys[key_ptr++] = map_id; + + TRY_PARSE_UNSIGNED_INTEGER( + if(tu64 > SECONDS_IN_A_DAY) + { + /* Larger than 1 day, so must be unix epoch time */ + Received->Telemetry.Time = tu64; + + /* Strip out date information. TODO: Find a good way of including this in the UKHAS ASCII */ + tu64 -= (uint64_t)(tu64 / SECONDS_IN_A_DAY) * SECONDS_IN_A_DAY; + } + else + { + /* Smaller than 1 day, either we've gone over 88mph, or it's seconds since midnight today */ + Received->Telemetry.Time = tu64 + (uint64_t)((uint64_t)time(NULL) / SECONDS_IN_A_DAY) * SECONDS_IN_A_DAY; + } + + int hours = tu64/(60*60); + tu64 = tu64 - (hours*60*60); + int mins = (tu64/60); + int secs = tu64-mins*60; + + snprintf(Received->Telemetry.TimeString, 9, "%02d:%02d:%02d",hours,mins,secs); + + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%02d:%02d:%02d",hours,mins,secs); + ) + else return 0; + + temp_ptr++; + break; + + case HABPACK_POSITION: + key_val[key_ptr] = temp_ptr; + keys[key_ptr++] = map_id; + + TRY_PARSE_ARRAY_AT_ONCE( + /* Check size of the array */ + if(array_size == 2 || array_size == 3) + { + /* Latitude - Signed Integer */ + if (!(cmp_read_sinteger(&cmp, &ts64))) + { + return 0; + } + + Received->Telemetry.Latitude = (double)ts64 / 10e6; + + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%lld,",ts64); + + /* Longitude - Signed Integer */ + if (!(cmp_read_sinteger(&cmp, &ts64))) + { + return 0; + } + + Received->Telemetry.Longitude = (double)ts64 / 10e6; + + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%lld,",ts64); + + /* Altitude (optional) - Signed Integer */ + Received->Telemetry.Altitude = 0.0; + if(array_size == 3) + { + if(!(cmp_read_sinteger(&cmp, &ts64))) + { + return 0; + } + + Received->Telemetry.Altitude = ts64; + + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%lld",ts64); + } + } + else return 0; + ) + else return 0; + + temp_ptr++; + break; + + case HABPACK_GNSS_SATELLITES: + key_val[key_ptr] = temp_ptr; + keys[key_ptr++] = map_id; + strncpy(field_name, "gnss_satellites", 31); + + TRY_PARSE_UNSIGNED_INTEGER( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%lld",tu64); + + HABPACK_TELEM_STORE_AS_INT64(tu64); + ) + else return 0; + + temp_ptr++; + break; + + case HABPACK_GNSS_LOCK: + key_val[key_ptr] = temp_ptr; + keys[key_ptr++] = map_id; + strncpy(field_name, "gnss_lock_type", 31); + + TRY_PARSE_UNSIGNED_INTEGER( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%lld",tu64); + + HABPACK_TELEM_STORE_AS_INT64(tu64); + ) + else return 0; + + temp_ptr++; + break; + + case HABPACK_VOLTAGE: + key_val[key_ptr] = temp_ptr; + keys[key_ptr++] = map_id; + strncpy(field_name, "voltage", 31); + + TRY_PARSE_FLOAT( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%f",tf32); + + HABPACK_TELEM_STORE_AS_DOUBLE(tf32); + ) + else TRY_PARSE_DOUBLE( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%f",tf64); + + HABPACK_TELEM_STORE_AS_DOUBLE(tf64); + ) + else TRY_PARSE_UNSIGNED_INTEGER( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%lld",tu64); + + HABPACK_TELEM_STORE_AS_DOUBLE((double)tu64 / 1000.0); // Scale for mV + ) + else TRY_PARSE_SIGNED_INTEGER( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%lld",ts64); + + HABPACK_TELEM_STORE_AS_DOUBLE((double)ts64 / 1000.0); // Scale for mV + ) + else TRY_PARSE_ARRAY( + snprintf(field_name, 31, "voltage%d", j); + + TRY_PARSE_FLOAT( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%f",tf32); + + HABPACK_TELEM_STORE_AS_DOUBLE(tf32); + ) + else TRY_PARSE_DOUBLE( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%f",tf64); + + HABPACK_TELEM_STORE_AS_DOUBLE(tf64); + ) + else TRY_PARSE_UNSIGNED_INTEGER( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%lld",tu64); + + HABPACK_TELEM_STORE_AS_DOUBLE((double)tu64 / 1000.0); // Scale for mV + ) + TRY_PARSE_SIGNED_INTEGER( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%lld",ts64); + + HABPACK_TELEM_STORE_AS_DOUBLE((double)ts64 / 1000.0); // Scale for mV + ) + else return 0; + + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,","); + ) + else return 0; + + temp_ptr++; + break; + + case HABPACK_INTERNAL_TEMPERATURE: + key_val[key_ptr] = temp_ptr; + keys[key_ptr++] = map_id; + strncpy(field_name, "internal_temperature", 31); + + TRY_PARSE_FLOAT( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%f",tf32); + + HABPACK_TELEM_STORE_AS_DOUBLE(tf32); + ) + else TRY_PARSE_DOUBLE( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%f",tf64); + + HABPACK_TELEM_STORE_AS_DOUBLE(tf64); + ) + else TRY_PARSE_UNSIGNED_INTEGER( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%lld",tu64); + + HABPACK_TELEM_STORE_AS_DOUBLE((double)tu64 / 1000.0); // Scale for milli-degrees + ) + else TRY_PARSE_SIGNED_INTEGER( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%lld",ts64); + + HABPACK_TELEM_STORE_AS_DOUBLE((double)ts64 / 1000.0); // Scale for milli-degrees + ) + else TRY_PARSE_ARRAY( + snprintf(field_name, 31, "internal_temperature%d", j); + + TRY_PARSE_FLOAT( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%f",tf32); + + HABPACK_TELEM_STORE_AS_DOUBLE(tf32); + ) + else TRY_PARSE_DOUBLE( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%f",tf64); + + HABPACK_TELEM_STORE_AS_DOUBLE(tf64); + ) + else TRY_PARSE_UNSIGNED_INTEGER( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%lld",tu64); + + HABPACK_TELEM_STORE_AS_DOUBLE((double)tu64 / 1000.0); // Scale for milli-degrees + ) + TRY_PARSE_SIGNED_INTEGER( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%lld",ts64); + + HABPACK_TELEM_STORE_AS_DOUBLE((double)ts64 / 1000.0); // Scale for milli-degrees + ) + else return 0; + + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,","); + ) + else return 0; + + temp_ptr++; + break; + + case HABPACK_EXTERNAL_TEMPERATURE: + key_val[key_ptr] = temp_ptr; + keys[key_ptr++] = map_id; + strncpy(field_name, "external_temperature", 31); + + TRY_PARSE_FLOAT( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%f",tf32); + + HABPACK_TELEM_STORE_AS_DOUBLE(tf32); + ) + else TRY_PARSE_DOUBLE( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%f",tf64); + + HABPACK_TELEM_STORE_AS_DOUBLE(tf64); + ) + else TRY_PARSE_UNSIGNED_INTEGER( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%lld",tu64); + + HABPACK_TELEM_STORE_AS_DOUBLE((double)tu64 / 1000.0); // Scale for milli-degrees + ) + else TRY_PARSE_SIGNED_INTEGER( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%lld",ts64); + + HABPACK_TELEM_STORE_AS_DOUBLE((double)ts64 / 1000.0); // Scale for milli-degrees + ) + else TRY_PARSE_ARRAY( + snprintf(field_name, 31, "external_temperature%d", j); + + TRY_PARSE_FLOAT( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%f",tf32); + + HABPACK_TELEM_STORE_AS_DOUBLE(tf32); + ) + else TRY_PARSE_DOUBLE( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%f",tf64); + + HABPACK_TELEM_STORE_AS_DOUBLE(tf64); + ) + else TRY_PARSE_UNSIGNED_INTEGER( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%lld",tu64); + + HABPACK_TELEM_STORE_AS_DOUBLE((double)tu64 / 1000.0); // Scale for milli-degrees + ) + TRY_PARSE_SIGNED_INTEGER( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%lld",ts64); + + HABPACK_TELEM_STORE_AS_DOUBLE((double)ts64 / 1000.0); // Scale for milli-degrees + ) + else return 0; + + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,","); + ) + else return 0; + + temp_ptr++; + break; + + case HABPACK_PRESSURE: + key_val[key_ptr] = temp_ptr; + keys[key_ptr++] = map_id; + strncpy(field_name, "pressure", 31); + + TRY_PARSE_FLOAT( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%f",tf32); + + HABPACK_TELEM_STORE_AS_DOUBLE(tf32); + ) + else TRY_PARSE_DOUBLE( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%f",tf64); + + HABPACK_TELEM_STORE_AS_DOUBLE(tf64); + ) + else TRY_PARSE_UNSIGNED_INTEGER( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%lld",tu64); + + HABPACK_TELEM_STORE_AS_DOUBLE((double)tu64 / 1000.0); // Scale for milli-bar + ) + else TRY_PARSE_ARRAY( + snprintf(field_name, 31, "pressure%d", j); + + TRY_PARSE_FLOAT( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%f",tf32); + + HABPACK_TELEM_STORE_AS_DOUBLE(tf32); + ) + else TRY_PARSE_DOUBLE( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%f",tf64); + + HABPACK_TELEM_STORE_AS_DOUBLE(tf64); + ) + TRY_PARSE_UNSIGNED_INTEGER( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%lld",tu64); + + HABPACK_TELEM_STORE_AS_DOUBLE((double)tu64 / 1000.0); // Scale for milli-bar + ) + else return 0; + + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,","); + ) + else return 0; + + temp_ptr++; + break; + + case HABPACK_RELATIVE_HUMIDITY: + key_val[key_ptr] = temp_ptr; + keys[key_ptr++] = map_id; + strncpy(field_name, "relative_humidity", 31); + + TRY_PARSE_FLOAT( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%f",tf32); + + HABPACK_TELEM_STORE_AS_DOUBLE(tf32); + ) + else TRY_PARSE_DOUBLE( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%f",tf64); + + HABPACK_TELEM_STORE_AS_DOUBLE(tf64); + ) + else TRY_PARSE_UNSIGNED_INTEGER( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%lld",tu64); + + HABPACK_TELEM_STORE_AS_DOUBLE(tu64); + ) + else TRY_PARSE_ARRAY( + snprintf(field_name, 31, "relative_humidity%d", j); + + TRY_PARSE_FLOAT( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%f",tf32); + + HABPACK_TELEM_STORE_AS_DOUBLE(tf32); + ) + else TRY_PARSE_DOUBLE( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%f",tf64); + + HABPACK_TELEM_STORE_AS_DOUBLE(tf64); + ) + TRY_PARSE_UNSIGNED_INTEGER( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%lld",tu64); + + HABPACK_TELEM_STORE_AS_DOUBLE(tu64); + ) + else return 0; + + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,","); + ) + else return 0; + + temp_ptr++; + break; + + case HABPACK_ABSOLUTE_HUMIDITY: + key_val[key_ptr] = temp_ptr; + keys[key_ptr++] = map_id; + strncpy(field_name, "absolute_humidity", 31); + + TRY_PARSE_FLOAT( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%f",tf32); + + HABPACK_TELEM_STORE_AS_DOUBLE(tf32); + ) + else TRY_PARSE_DOUBLE( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%f",tf64); + + HABPACK_TELEM_STORE_AS_DOUBLE(tf64); + ) + else TRY_PARSE_UNSIGNED_INTEGER( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%lld",tu64); + + HABPACK_TELEM_STORE_AS_DOUBLE((double)tu64 / 1000.0); // Scale for milli-grams / m^3 + ) + else TRY_PARSE_ARRAY( + snprintf(field_name, 31, "absolute_humidity%d", j); + + TRY_PARSE_FLOAT( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%f",tf32); + + HABPACK_TELEM_STORE_AS_DOUBLE(tf32); + ) + else TRY_PARSE_DOUBLE( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%f",tf64); + + HABPACK_TELEM_STORE_AS_DOUBLE(tf64); + ) + TRY_PARSE_UNSIGNED_INTEGER( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%lld",tu64); + + HABPACK_TELEM_STORE_AS_DOUBLE((double)tu64 / 1000.0); // Scale for milli-grams / m^3 + ) + else return 0; + + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,","); + ) + else return 0; + + temp_ptr++; + break; + + case HABPACK_DOWNLINK_FREQUENCY: + strncpy(field_name, "downlink_frequency", 31); + + TRY_PARSE_UNSIGNED_INTEGER( + Received->isCallingBeacon = true; + Received->Telemetry.DownlinkFrequency = tu64; + + HABPACK_TELEM_STORE_AS_INT64(tu64); + ) + else return 0; + + temp_ptr++; + break; + + case HABPACK_DOWNLINK_LORA_MODE: + strncpy(field_name, "downlink_lora_mode", 31); + + TRY_PARSE_UNSIGNED_INTEGER( + Received->Telemetry.DownlinkLoraMode = tu64; + + HABPACK_TELEM_STORE_AS_INT64(tu64); + ) + else return 0; + + temp_ptr++; + break; + + case HABPACK_DOWNLINK_LORA_IMPLICIT: + strncpy(field_name, "downlink_lora_implicit", 31); + + TRY_PARSE_UNSIGNED_INTEGER( + Received->Telemetry.DownlinkLoraMode = -1; + Received->Telemetry.DownlinkLoraImplicit = tu64; + + HABPACK_TELEM_STORE_AS_INT64(tu64); + ) + else return 0; + + temp_ptr++; + break; + + case HABPACK_DOWNLINK_LORA_ERRORCODING: + strncpy(field_name, "downlink_lora_error_coding", 31); + + TRY_PARSE_UNSIGNED_INTEGER( + Received->Telemetry.DownlinkLoraMode = -1; + Received->Telemetry.DownlinkLoraErrorCoding = tu64; + + HABPACK_TELEM_STORE_AS_INT64(tu64); + ) + else return 0; + + temp_ptr++; + break; + + case HABPACK_DOWNLINK_LORA_BANDWIDTH: + strncpy(field_name, "downlink_lora_bandwidth", 31); + + TRY_PARSE_UNSIGNED_INTEGER( + Received->Telemetry.DownlinkLoraMode = -1; + + /* Convert integer ID to float bandwidth. Ref: https://ukhas.org.uk/communication:habpack */ + switch(tu64) + { + case 0: + tf64 = 7.8; + break; + case 1: + tf64 = 10.4; + break; + case 2: + tf64 = 15.6; + break; + case 3: + tf64 = 20.8; + break; + case 4: + tf64 = 31.25; + break; + case 5: + tf64 = 41.7; + break; + case 6: + tf64 = 62.5; + break; + case 7: + tf64 = 125; + break; + case 8: + tf64 = 250; + break; + case 9: + tf64 = 500; + break; + default: + return 0; + } + Received->Telemetry.DownlinkLoraBandwidth = tf64; + + HABPACK_TELEM_STORE_AS_DOUBLE(tf64); + ) + else return 0; + + temp_ptr++; + break; + + case HABPACK_DOWNLINK_LORA_SPREADING: + strncpy(field_name, "downlink_lora_spreading_factor", 31); + + TRY_PARSE_UNSIGNED_INTEGER( + Received->Telemetry.DownlinkLoraMode = -1; + Received->Telemetry.DownlinkLoraSpreadingFactor = tu64; + + HABPACK_TELEM_STORE_AS_INT64(tu64); + ) + else return 0; + + temp_ptr++; + break; + + case HABPACK_DOWNLINK_LORA_LDO: + strncpy(field_name, "downlink_lora_ldo_on", 31); + + TRY_PARSE_UNSIGNED_INTEGER( + Received->Telemetry.DownlinkLoraMode = -1; + Received->Telemetry.DownlinkLoraLowDatarateOptimise = tu64; + + HABPACK_TELEM_STORE_AS_INT64(tu64); + ) + else return 0; + + temp_ptr++; + break; + + case HABPACK_UPLINK_COUNT: + key_val[key_ptr] = temp_ptr; + keys[key_ptr++] = map_id; + strncpy(field_name, "uplink_count", 31); + + TRY_PARSE_UNSIGNED_INTEGER( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%lld",tu64); + + HABPACK_TELEM_STORE_AS_INT64(tu64); + ) + else return 0; + + temp_ptr++; + break; + + case HABPACK_PREDICTED_LANDING_TIME: + key_val[key_ptr] = temp_ptr; + keys[key_ptr++] = map_id; + strncpy(field_name, "predicted_landing_time", 31); + + TRY_PARSE_UNSIGNED_INTEGER( + if(tu64 > SECONDS_IN_A_DAY) + { + /* Larger than 1 day, so must be unix epoch time */ + HABPACK_TELEM_STORE_AS_INT64(tu64); + + /* Strip out date information. TODO: Find a good way of including this in the UKHAS ASCII */ + tu64 -= (uint64_t)(tu64 / SECONDS_IN_A_DAY) * SECONDS_IN_A_DAY; + } + else + { + /* Smaller than 1 day, either we've gone over 88mph, or it's seconds since midnight today */ + HABPACK_TELEM_STORE_AS_INT64(tu64 + (uint64_t)((uint64_t)time(NULL) / SECONDS_IN_A_DAY) * SECONDS_IN_A_DAY); + } + + int hours = tu64/(60*60); + tu64 = tu64 - (hours*60*60); + int mins = (tu64/60); + int secs = tu64-mins*60; + + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%02d:%02d:%02d",hours,mins,secs); + ) + else return 0; + + temp_ptr++; + break; + + case HABPACK_PREDICTED_LANDING_POSITION: + key_val[key_ptr] = temp_ptr; + keys[key_ptr++] = map_id; + + TRY_PARSE_ARRAY_AT_ONCE( + /* Check size of the array */ + if(array_size == 2 || array_size == 3) + { + /* Latitude - Signed Integer */ + if (!(cmp_read_sinteger(&cmp, &ts64))) + { + return 0; + } + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%lld,",ts64); + + strncpy(field_name, "predicted_landing_latitude", 31); + HABPACK_TELEM_STORE_AS_DOUBLE((double)ts64 / 10e6); + + /* Longitude - Signed Integer */ + if (!(cmp_read_sinteger(&cmp, &ts64))) + { + return 0; + } + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%lld,",ts64); + + strncpy(field_name, "predicted_landing_longitude", 31); + HABPACK_TELEM_STORE_AS_DOUBLE((double)ts64 / 10e6); + + /* Altitude (optional) - Signed Integer */ + if(array_size == 3) + { + if(!(cmp_read_sinteger(&cmp, &ts64))) + { + return 0; + } + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%lld",ts64); + + strncpy(field_name, "predicted_landing_altitude", 31); + HABPACK_TELEM_STORE_AS_INT64(ts64); + } + } + else return 0; + ) + else return 0; + + temp_ptr++; + break; + + default: // Unknown + TRY_PARSE_STRING( + key_val[key_ptr] = temp_ptr; + keys[key_ptr++] = map_id; + + temp_remaining = 1024-temp_ptr; + while(temp[temp_ptr]) + temp_ptr++; + + /* TODO: Store in habpack data list */ + + temp_ptr++; + ) + else TRY_PARSE_UNSIGNED_INTEGER( + key_val[key_ptr] = temp_ptr; + keys[key_ptr++] = map_id; + + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%lld",tu64); + + snprintf(field_name, 31, "%lld", map_id); + HABPACK_TELEM_STORE_AS_INT64(tu64); + + temp_ptr++; + ) + else TRY_PARSE_SIGNED_INTEGER( + key_val[key_ptr] = temp_ptr; + keys[key_ptr++] = map_id; + + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%lld",ts64); + + snprintf(field_name, 31, "%lld", map_id); + HABPACK_TELEM_STORE_AS_INT64(ts64); + + temp_ptr++; + ) + else TRY_PARSE_FLOAT( + key_val[key_ptr] = temp_ptr; + keys[key_ptr++] = map_id; + + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%f",tf32); + + snprintf(field_name, 31, "%lld", map_id); + HABPACK_TELEM_STORE_AS_DOUBLE(tf32); + + temp_ptr++; + ) + else TRY_PARSE_DOUBLE( + key_val[key_ptr] = temp_ptr; + keys[key_ptr++] = map_id; + + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%f",tf64); + + snprintf(field_name, 31, "%lld", map_id); + HABPACK_TELEM_STORE_AS_DOUBLE(tf64); + + temp_ptr++; + ) + else TRY_PARSE_ARRAY_AT_ONCE( + key_val[key_ptr] = temp_ptr; + keys[key_ptr++] = map_id; + + for (j = 0; j < array_size; j++) + { + /* Read array element */ + if(!cmp_read_object(&cmp, &item)) + return 0; + + TRY_PARSE_STRING( + while(temp[temp_ptr]) + temp_ptr++; + ) + else TRY_PARSE_UNSIGNED_INTEGER( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%lld",tu64); + + snprintf(field_name, 31, "%lld.%d", map_id, j); + HABPACK_TELEM_STORE_AS_INT64(tu64); + ) + else TRY_PARSE_SIGNED_INTEGER( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%lld",ts64); + + snprintf(field_name, 31, "%lld.%d", map_id, j); + HABPACK_TELEM_STORE_AS_INT64(ts64); + ) + else TRY_PARSE_FLOAT( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%f",tf32); + + snprintf(field_name, 31, "%lld.%d", map_id, j); + HABPACK_TELEM_STORE_AS_DOUBLE(tf32); + ) + else TRY_PARSE_DOUBLE( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%f",tf64); + + snprintf(field_name, 31, "%lld.%d", map_id, j); + HABPACK_TELEM_STORE_AS_DOUBLE(tf64); + ) + else TRY_PARSE_ARRAY_AT_ONCE_NESTED( + for (k = 0; k < array_size2; k++) + { + /* Read array element */ + if(!cmp_read_object(&cmp, &item)) + return 0; + + TRY_PARSE_STRING( + while(temp[temp_ptr]) + temp_ptr++; + ) + else TRY_PARSE_UNSIGNED_INTEGER( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%lld",tu64); + + snprintf(field_name, 31, "%lld.%d.%d", map_id, j, k); + HABPACK_TELEM_STORE_AS_INT64(tu64); + ) + else TRY_PARSE_SIGNED_INTEGER( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%lld",ts64); + + snprintf(field_name, 31, "%lld.%d.%d", map_id, j, k); + HABPACK_TELEM_STORE_AS_INT64(ts64); + ) + else TRY_PARSE_FLOAT( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%f",tf32); + + snprintf(field_name, 31, "%lld.%d.%d", map_id, j, k); + HABPACK_TELEM_STORE_AS_DOUBLE(tf32); + ) + else TRY_PARSE_DOUBLE( + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,"%f",tf64); + + snprintf(field_name, 31, "%lld.%d.%d", map_id, j, k); + HABPACK_TELEM_STORE_AS_DOUBLE(tf64); + ) + else return 0; + + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,","); + } + + /* Replace last comma with null */ + temp[temp_ptr] = '\0'; + ) + else return 0; + + temp_remaining = 1024-temp_ptr; + temp_ptr += snprintf(&temp[temp_ptr],temp_remaining,","); + } + + /* Replace last comma with null */ + temp[temp_ptr] = '\0'; + ) + break; + } + } + + uint16_t crc; + + if (key_ptr == 0) + return 0; + + out_ptr += snprintf(Received->UKHASstring,UKHASstring_length,"$$"); + + //now order the items into the output string + + int copy_ptr = 0; + + for (i=0; i < 128; i++) { + for (j=0; j < key_ptr; j++) { + if (keys[j] == i){ + copy_ptr = key_val[j]; + if (temp[copy_ptr]){ + while((out_ptr < UKHASstring_length-1) && (temp[copy_ptr])){ + Received->UKHASstring[out_ptr++] = temp[copy_ptr++]; + } + Received->UKHASstring[out_ptr++] = ','; + } + } + } + } + out_ptr--; + Received->UKHASstring[out_ptr] = '\0'; + + /* Base64 encoding of original packet - TODO: Flag? + size_t out_len; + base64_encode(Received->Message, Received->Bytes, &out_len, &Received->UKHASstring[out_ptr]); + out_ptr += out_len; + Received->UKHASstring[out_ptr] = '\0'; + */ + + // add checksum to keep everything happy + crc = CRC16((unsigned char *)&Received->UKHASstring[2]); + snprintf(&Received->UKHASstring[out_ptr],UKHASstring_length-out_ptr-7,"*%04x\n",crc); + + return 1; +} \ No newline at end of file diff --git a/habpack.h b/habpack.h new file mode 100644 index 0000000..a074b68 --- /dev/null +++ b/habpack.h @@ -0,0 +1,48 @@ +#ifndef __HABPACK_H__ +#define __HABPACK_H__ + +#include "global.h" + +/* Core Telemetry */ +#define HABPACK_CALLSIGN 0 +#define HABPACK_SENTENCE_ID 1 +#define HABPACK_TIME 2 +#define HABPACK_POSITION 3 +#define HABPACK_GNSS_SATELLITES 4 +#define HABPACK_GNSS_LOCK 5 +#define HABPACK_VOLTAGE 6 + +/* Environmental Telemetry */ +#define HABPACK_INTERNAL_TEMPERATURE 10 +#define HABPACK_EXTERNAL_TEMPERATURE 11 +#define HABPACK_PRESSURE 12 +#define HABPACK_RELATIVE_HUMIDITY 13 +#define HABPACK_ABSOLUTE_HUMIDITY 14 + +/* Calling Beacon */ +#define HABPACK_DOWNLINK_FREQUENCY 20 +/* Common LoRa Mode */ +#define HABPACK_DOWNLINK_LORA_MODE 21 +/* _or_ Custom LoRa Parameters */ +#define HABPACK_DOWNLINK_LORA_IMPLICIT 22 +#define HABPACK_DOWNLINK_LORA_ERRORCODING 23 +#define HABPACK_DOWNLINK_LORA_BANDWIDTH 24 +#define HABPACK_DOWNLINK_LORA_SPREADING 25 +#define HABPACK_DOWNLINK_LORA_LDO 26 + +/* Uplink */ +#define HABPACK_UPLINK_COUNT 30 + +/* Landing Prediction */ +#define HABPACK_PREDICTED_LANDING_TIME 40 +#define HABPACK_PREDICTED_LANDING_POSITION 41 + +/* Multi-Position. TODO: Parsing unimplemented */ +#define HABPACK_MULTI_POS_POSITION_SCALING 60 +#define HABPACK_MULTI_POS_ALTITUDE_SCALING 61 +#define HABPACK_MULTI_POS_ARRAY 62 + +int Habpack_Process_Message(received_t *Received); +void Habpack_Telem_Destroy(received_t *Received); + +#endif /* __HABPACK_H__ */ \ No newline at end of file diff --git a/server.c b/server.c index 63007e7..035325c 100644 --- a/server.c +++ b/server.c @@ -52,7 +52,7 @@ void ProcessJSONClientLine(int connfd, char *line) if (strchr(line, '=') != NULL) { // Setting - char *setting, *value, *saveptr; + char *setting, *value, *saveptr = NULL; setting = strtok_r(line, "=", &saveptr); value = strtok_r( NULL, "\n", &saveptr); @@ -62,7 +62,7 @@ void ProcessJSONClientLine(int connfd, char *line) else if (strchr(line, ':') != NULL) { // Command with parameters - char *command, *value, *saveptr; + char *command, *value, *saveptr = NULL; command = strtok_r(line, ":", &saveptr); value = strtok_r(NULL, "\n", &saveptr);