From d3598381289ac23217c472d5f48df3bea891ecf2 Mon Sep 17 00:00:00 2001 From: kartofen Date: Mon, 24 Mar 2025 03:08:00 +0200 Subject: first --- msgpack.c | 267 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 267 insertions(+) create mode 100644 msgpack.c (limited to 'msgpack.c') diff --git a/msgpack.c b/msgpack.c new file mode 100644 index 0000000..b5ba7ae --- /dev/null +++ b/msgpack.c @@ -0,0 +1,267 @@ +#include "msgpack.h" +#include + +#define I8(b) (*(int8_t*)(b)) +#define I16(b) ((int16_t)be16toh(*(uint16_t *)(b))) +#define I32(b) ((int32_t)be32toh(*(uint32_t *)(b))) +#define I64(b) ((int64_t)be64toh(*(uint64_t *)(b))) + +#define U8(b) (*(uint8_t*)(b)) +#define U16(b) ((uint16_t)be16toh(*(uint16_t *)(b))) +#define U32(b) ((uint32_t)be32toh(*(uint32_t *)(b))) +#define U64(b) ((uint64_t)be64toh(*(uint64_t *)(b))) + +#define ENOUGH_BYTES(pack, n, on_fail) if((pack)->size < (n)) { on_fail; } + +// range low, range high +#define RANGES(X) \ + X(0x00, 0x7F) \ + X(0x80, 0x8F) \ + X(0x90, 0x9F) \ + X(0xA0, 0xBF) \ + X(0xE0, 0xFF) + +#define FORMAT_NIL(X) \ + X(0xC1, FMT_UNKNOWN, MSGPACK_UNKNOWN, ;, 1, 0) \ + X(0xC0, FMT_NIL, MSGPACK_NIL, ;, 1, 0) + +// byte, fmt, type, _, offset, length, data +#define FORMAT_BOOL(X) \ + X(0xC2, FMT_BOOL_TRUE, MSGPACK_BOOL, ;, 1, 0, 1) \ + X(0xC3, FMT_BOOL_FALSE, MSGPACK_BOOL, ;, 1, 0, 0) + +// byte, fmt, type, subtype, offset, length, data +#define FORMAT_INT(X) \ + X(0x00, FMT_FIX_UINT, MSGPACK_INT, MSGPACK_INT_UNSIGNED, 1, 0, m->u = U8(pack->bin)) \ + X(0XE0, FMT_FIX_INT, MSGPACK_INT, MSGPACK_INT_SIGNED, 1, 0, m->i = I8(pack->bin)) \ + X(0xCC, FMT_U8, MSGPACK_INT, MSGPACK_INT_UNSIGNED, 1, 1, m->u = U8(pack->bin+1)) \ + X(0xCD, FMT_U16, MSGPACK_INT, MSGPACK_INT_UNSIGNED, 1, 2, m->u = U16(pack->bin+1)) \ + X(0xCE, FMT_U32, MSGPACK_INT, MSGPACK_INT_UNSIGNED, 1, 4, m->u = U32(pack->bin+1)) \ + X(0xCF, FMT_U64, MSGPACK_INT, MSGPACK_INT_UNSIGNED, 1, 8, m->u = U64(pack->bin+1)) \ + X(0xD0, FMT_I8, MSGPACK_INT, MSGPACK_INT_SIGNED, 1, 1, m->i = I8(pack->bin+1)) \ + X(0xD1, FMT_I16, MSGPACK_INT, MSGPACK_INT_SIGNED, 1, 2, m->i = I16(pack->bin+1)) \ + X(0xD2, FMT_I32, MSGPACK_INT, MSGPACK_INT_SIGNED, 1, 4, m->i = I32(pack->bin+1)) \ + X(0xD3, FMT_I64, MSGPACK_INT, MSGPACK_INT_SIGNED, 1, 8, m->i = I64(pack->bin+1)) \ + +// byte, fmt, type, subtype, offset, length, data +#define FORMAT_FLOAT(X) \ + X(0xCA, FMT_FLOAT, MSGPACK_FLOAT, MSGPACK_FLOAT_32, 1, 4, m->_u = U32(pack->bin+1)) \ + X(0xCB, FMT_DOUBLE, MSGPACK_FLOAT, MSGPACK_FLOAT_64, 1, 8, m->_u = U64(pack->bin+1)) \ + +// byte, fmt, type, subtype, offset, length +#define FORMAT_RAW(X) \ + X(0xA0, FMT_FIX_STR, MSGPACK_RAW, MSGPACK_RAW_STRING, 1, pack->bin[0] & 1) \ + X(0xC4, FMT_BIN8, MSGPACK_RAW, MSGPACK_RAW_BIN, 1, pack->bin[1]) \ + X(0xC5, FMT_BIN16, MSGPACK_RAW, MSGPACK_RAW_BIN, 3, U16(pack->bin+1)) \ + X(0xC6, FMT_BIN32, MSGPACK_RAW, MSGPACK_RAW_BIN, 5, U32(pack->bin+1)) \ + X(0xD9, FMT_STR8, MSGPACK_RAW, MSGPACK_RAW_STRING, 1, pack->bin[1]) \ + X(0xDA, FMT_STR16, MSGPACK_RAW, MSGPACK_RAW_STRING, 3, U16(pack->bin+1)) \ + X(0xDB, FMT_STR32, MSGPACK_RAW, MSGPACK_RAW_STRING, 5, U32(pack->bin+1)) + +// byte, fmt, type, elements, offset, lenght +#define FORMAT_ARRAY(X) \ + X(0x90, FMT_FIX_ARRAY, MSGPACK_ARRAY, pack->bin[0] & 0x0F, 1, elements_length(pack, fmt)) \ + X(0xDC, FMT_ARRAY16, MSGPACK_ARRAY, U16(pack->bin+1), 3, elements_length(pack, fmt)) \ + X(0xDD, FMT_ARRAY32, MSGPACK_ARRAY, U32(pack->bin+1), 5, elements_length(pack, fmt)) +#define FORMAT_MAP(X) \ + X(0x80, FMT_FIX_MAP, MSGPACK_MAP, 2 * (pack->bin[0] & 0x0F), 1, elements_length(pack, fmt)) \ + X(0xDE, FMT_MAP16, MSGPACK_MAP, 2 * (U16(pack->bin+1)), 3, elements_length(pack, fmt)) \ + X(0xDF, FMT_MAP32, MSGPACK_MAP, 2 * (U16(pack->bin+1)), 5, elements_length(pack, fmt)) + +#define FORMAT_EXT(X) + +#define FORMATS(X) \ + FORMAT_NIL (X) \ + FORMAT_BOOL (X) \ + FORMAT_INT (X) \ + FORMAT_FLOAT(X) \ + FORMAT_RAW (X) \ + FORMAT_ARRAY(X) \ + FORMAT_MAP (X) \ + FORMAT_EXT (X) + +#define EXPAND(X) X +#define X_FMT_ENUM(_byte, fmt, ...) fmt, +#define X_FMT_TYPE(_byte, fmt, type, ...) [fmt] = type, +#define X_BYTE_FMT( byte, fmt, ...) [byte] = fmt, + +enum msgpack_fmt { + FORMATS(X_FMT_ENUM) +}; + +static const enum msgpack_type fmt_to_type[] = { + FORMATS(X_FMT_TYPE) +}; +static const enum msgpack_fmt byte_to_fmt[] = { + FORMATS(X_BYTE_FMT) +}; + +static enum msgpack_fmt pack_fmt(const msgpack_t *pack); +static size_t elements_length(msgpack_t *pack, enum msgpack_fmt fmt); +static size_t pack_size(msgpack_t *pack); + + +#define SUCCESS(type) SUCCESS2(type, 0) +#define ERROR(type, err) ERROR2(type, err, 0) +#define SUCCESS2(type, rest) \ + ((rest << 6) | (type)) +#define ERROR2(type, err, rest) \ + ((1 << (sizeof(int)*8-1))| (rest << 6) | (err << 3) | (type)) + +#define CHECK_READ_ARGS(type, pack, m) \ + do { \ + if(!(pack) || !(pack)->bin || (pack)->size == 0) \ + return ERROR((type), MSGPACK_ERROR_INVALID_PACK); \ + if((pack)->bin[0] == 0xc) \ + return ERROR2((type), MSGPACK_ERROR_WRONG_TYPE, MSGPACK_NIL); \ + if(!(m)) \ + return ERROR((type), MSGPACK_ERROR_INVALID_ARGUMENT); \ + } while(0) + + + +#define X_READ_INT(_byte, x_fmt, x_type, x_subtype, x_offset, x_length, x_data) \ + case x_fmt: { \ + ENOUGH_BYTES(pack, (x_offset) + (x_length), \ + return ERROR2(x_type, MSGPACK_ERROR_UNEXPECTED_END, x_subtype)); \ + x_data; \ + return SUCCESS2(x_type, x_subtype); \ + } + +int msgpack_read_int(const msgpack_t *pack, union mp_int *m) +{ + CHECK_READ_ARGS(MSGPACK_INT, pack, m); + + enum msgpack_fmt fmt = pack_fmt(pack); + + switch(fmt) { + FORMAT_INT(X_READ_INT) + default: + return ERROR2(MSGPACK_INT, MSGPACK_ERROR_WRONG_TYPE, fmt_to_type[fmt]); + } +} + +#define X_READ_BOOL(_byte, x_fmt, x_type, _, _offset, _length, x_data) \ + case x_fmt: { \ + *m = x_data; \ + return SUCCESS(x_type); \ + } +int msgpack_read_bool(const msgpack_t *pack, int *m) +{ + CHECK_READ_ARGS(MSGPACK_BOOL, pack, m); + + enum msgpack_fmt fmt = pack_fmt(pack); + + switch(fmt) { + FORMAT_BOOL(X_READ_BOOL) + default: + return ERROR2(MSGPACK_BOOL, MSGPACK_ERROR_WRONG_TYPE, fmt_to_type[fmt]); + } +} + +#define X_READ_FLOAT(...) X_READ_INT(__VA_ARGS__) + +int msgpack_read_float(const msgpack_t *pack, union mp_float *m) +{ + CHECK_READ_ARGS(MSGPACK_FLOAT, pack, m); + + enum msgpack_fmt fmt = pack_fmt(pack); + + switch(fmt) { + FORMAT_FLOAT(X_READ_FLOAT) + default: + return ERROR2(MSGPACK_FLOAT, MSGPACK_ERROR_WRONG_TYPE, fmt_to_type[fmt]); + } +} + +int msgpack_read_raw (const msgpack_t *pack, struct mp_bin *m); +int msgpack_read_raw_cpy (const msgpack_t *pack, struct mp_bin *m); +int msgpack_read_ext (const msgpack_t *pack, struct mp_bin *m); +int msgpack_read_ext_cpy (const msgpack_t *pack, struct mp_bin *m); + +int msgpack_read_array (msgpack_t *pack, size_t *length); +int msgpack_read_map (msgpack_t *pack, size_t *length); +int msgpack_read_array2 (const msgpack_t *pack, struct mp_array *m); +int msgpack_read_map2 (const msgpack_t *pack, struct mp_map *m); + + +int msgpack_write_int (msgpack_t *pack, const union mp_int *m, int subtype); +int msgpack_write_bool (msgpack_t *pack, const int *m); +int msgpack_write_float (msgpack_t *pack, const union mp_float *m, int subtype); + +int msgpack_write_raw (msgpack_t *pack, const struct mp_bin *m, int subtype); +int msgpack_write_ext (msgpack_t *pack, const struct mp_bin *m, int subtype); + +int msgpack_write_array (msgpack_t *pack, const size_t *length); +int msgpack_write_map (msgpack_t *pack, const size_t *length); + + +#define X_COMPLEX_LENGTH(_byte, x_fmt, _type, x_elements, x_offset, x_length) \ + case x_fmt: { \ + ENOUGH_BYTES(pack, (x_offset), return 0); \ + offset = (x_offset); \ + elements = (x_elements); \ + } break; + +static size_t elements_length(msgpack_t *pack, enum msgpack_fmt fmt) +{ + size_t offset = 0; + size_t elements = 0; + + switch(fmt) { + FORMAT_ARRAY(X_COMPLEX_LENGTH) + FORMAT_MAP(X_COMPLEX_LENGTH) + default: return 0; + } + + for(size_t i = 0; i < elements; i++) { + msgpack_t new_pack = {pack->bin + offset, + pack->size - offset, pack->membuf}; + size_t len = pack_size(&new_pack); + if(len == 0) return 0; + + offset += len; + ENOUGH_BYTES(pack, offset, return 0); + } + + return offset; +} + +#define X_PACK_SIZE(_byte, x_fmt, _type, _, x_offset, x_length, ...) \ + case x_fmt: { \ + ENOUGH_BYTES(pack, (x_offset), return 0); \ + offset = (x_offset); \ + length = (x_length); \ + } break; + +static size_t pack_size(msgpack_t *pack) +{ + enum msgpack_fmt fmt = pack_fmt(pack); + + size_t offset = 0; + size_t length = 0; + + switch(fmt) { + FORMATS(X_PACK_SIZE) + default: return 0; + } + + if(fmt_to_type[fmt] == MSGPACK_ARRAY || fmt_to_type[fmt] == MSGPACK_MAP) + if(length == 0) return 0; + + return offset + length; +} + +#define X_MASK_RANGE(range_low, range_high) \ + if(byte >= range_low && byte <= range_high) \ + byte &= ~(uint8_t)(range_high-range_low); \ + else \ + +static enum msgpack_fmt pack_fmt(const msgpack_t *pack) +{ + ENOUGH_BYTES(pack, 1, return 0); + unsigned char byte = pack->bin[0]; + + RANGES(X_MASK_RANGE){}; + return byte_to_fmt[byte]; +} -- cgit v1.2.3