first

1 files changed, 267 insertions, 0 deletions

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 <endian.h>
+
+#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];
+}