mayhem-firmware/firmware/common/msgpack.cpp

/*
 * Copyright (C) 2015 Jared Boone, ShareBrained Technology, Inc.
 * Copyright (C) 2016 Furrtek
 *
 * This file is part of PortaPack.
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2, or (at your option)
 * any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

#include "msgpack.hpp"

bool MsgPack::get_bool(const void* buffer, const bool inc, bool* value) {
    uint8_t v;

    if (seek_ptr >= buffer_size) return false;  // End of buffer

    v = ((uint8_t*)buffer)[seek_ptr];
    if (v == MSGPACK_FALSE)
        *value = false;
    else if (v == MSGPACK_TRUE)
        *value = true;
    else
        return false;  // Not a bool

    if (inc) seek_ptr++;
    return true;
}

bool MsgPack::get_raw_byte(const void* buffer, const bool inc, uint8_t* byte) {
    if (seek_ptr >= buffer_size) return false;  // End of buffer
    *byte = ((uint8_t*)buffer)[seek_ptr];
    if (inc) seek_ptr++;
    return true;
}

bool MsgPack::get_raw_word(const void* buffer, const bool inc, uint16_t* word) {
    if ((seek_ptr + 1) >= buffer_size) return false;  // End of buffer
    *word = (((uint8_t*)buffer)[seek_ptr] << 8) | ((uint8_t*)buffer)[seek_ptr + 1];
    if (inc) seek_ptr += 2;
    return true;
}

bool MsgPack::get_u8(const void* buffer, const bool inc, uint8_t* value) {
    uint8_t v;

    if (seek_ptr >= buffer_size) return false;  // End of buffer

    v = ((uint8_t*)buffer)[seek_ptr];

    if (!(v & 0x80))
        *value = ((uint8_t*)buffer)[seek_ptr];  // Fixnum
    else if (v == MSGPACK_TYPE_U8)
        *value = ((uint8_t*)buffer)[seek_ptr + 1];  // u8
    else
        return false;  // Value isn't a u8 or fixnum

    if (inc) seek_ptr++;
    return true;
}

// TODO: Typecheck function

bool MsgPack::get_u16(const void* buffer, const bool inc, uint16_t* value) {
    uint8_t byte;

    if ((seek_ptr + 1) >= buffer_size) return false;                                      // End of buffer
    if ((get_raw_byte(buffer, true, &byte)) && (byte != MSGPACK_TYPE_U16)) return false;  // Value isn't a u16
    *value = (((uint8_t*)buffer)[seek_ptr] << 8) | ((uint8_t*)buffer)[seek_ptr + 1];
    if (inc) seek_ptr += 2;
    return true;
}

bool MsgPack::get_s32(const void* buffer, const bool inc, int32_t* value) {
    uint8_t byte;

    if ((seek_ptr + 3) >= buffer_size) return false;                                      // End of buffer
    if ((get_raw_byte(buffer, true, &byte)) && (byte != MSGPACK_TYPE_S32)) return false;  // Value isn't a s32
    *value = (((uint8_t*)buffer)[seek_ptr] << 24) | (((uint8_t*)buffer)[seek_ptr + 1] << 16) |
             (((uint8_t*)buffer)[seek_ptr + 2] << 8) | ((uint8_t*)buffer)[seek_ptr + 3];
    if (inc) seek_ptr += 4;
    return true;
}

bool MsgPack::get_string(const void* buffer, const bool inc, std::string& value) {
    size_t length;
    uint8_t byte;

    // Todo: Set max length !
    if ((seek_ptr + 3) >= buffer_size) return false;                                                                       // End of buffer
    if ((get_raw_byte(buffer, true, &byte)) && (byte != MSGPACK_TYPE_STR8) && (byte != MSGPACK_TYPE_STR16)) return false;  // Value isn't a str8 or str16

    if (byte == MSGPACK_TYPE_STR8) {
        if (!get_raw_byte(buffer, true, (uint8_t*)&length)) return false;  // Couldn't get str8 length
    } else if (byte == MSGPACK_TYPE_STR16) {
        if (!get_raw_word(buffer, true, (uint16_t*)&length)) return false;  // Couldn't get str16 length
    }

    memcpy(&value[0], ((uint8_t*)buffer), length);  // std::string(

    if (inc) seek_ptr += length;
    return true;
}

bool MsgPack::init_search(const void* buffer, const size_t size) {
    uint8_t byte;
    uint16_t map_size;

    if (!size) return false;
    buffer_size = size;
    seek_ptr = 0;
    if ((get_raw_byte(buffer, true, &byte)) && (byte != MSGPACK_TYPE_MAP16)) return false;  // First record isn't a map16
    if (!get_raw_word(buffer, true, &map_size)) return false;                               // Couldn't get map16 size
    if (!map_size) return false;

    return true;
}

bool MsgPack::skip(const void* buffer) {
    uint8_t byte, c;
    size_t length;

    if (!get_raw_byte(buffer, true, &byte)) return false;  // Couldn't get type

    if (!(byte & 0x80)) return true;         // Positive fixnum, already skipped by get_raw_byte
    if ((byte & 0xE0) == 0xE0) return true;  // Negative fixnum, already skipped by get_raw_byte
    if ((byte & 0xE0) == 0xA0) {             // Fixstr
        seek_ptr += (byte & 0x1F);
        return true;
    }
    if ((byte & 0xF0) == 0x80) {  // Fixmap
        length = (byte & 0x0F) * 2;
        for (c = 0; c < length; c++)
            skip(buffer);
        return true;
    }
    if ((byte & 0xF0) == 0x90) {  // Fixarray
        length = byte & 0x0F;
        for (c = 0; c < length; c++)
            skip(buffer);
        return true;
    }

    switch (byte) {
        case MSGPACK_NIL:
        case MSGPACK_FALSE:
        case MSGPACK_TRUE:  // Already skipped by get_raw_byte
            break;
        case MSGPACK_TYPE_U8:
        case MSGPACK_TYPE_S8:
            seek_ptr++;
            break;
        case MSGPACK_TYPE_U16:
        case MSGPACK_TYPE_S16:
            seek_ptr += 2;
            break;
        case MSGPACK_TYPE_U32:
        case MSGPACK_TYPE_S32:
            seek_ptr += 4;
            break;
        case MSGPACK_TYPE_U64:
        case MSGPACK_TYPE_S64:
            seek_ptr += 8;
            break;

        case MSGPACK_TYPE_STR8:
            if (!get_raw_byte(buffer, true, (uint8_t*)&length)) return false;  // Couldn't get str8 length
            seek_ptr += length;
            break;
        case MSGPACK_TYPE_STR16:
            if (!get_raw_word(buffer, true, (uint16_t*)&length)) return false;  // Couldn't get str16 length
            seek_ptr += length;
            break;

        case MSGPACK_TYPE_ARR16:
            if (!get_raw_word(buffer, true, (uint16_t*)&length)) return false;  // Couldn't get arr16 length
            for (c = 0; c < length; c++)
                skip(buffer);
            break;

        case MSGPACK_TYPE_MAP16:
            if (!get_raw_word(buffer, true, (uint16_t*)&length)) return false;  // Couldn't get map16 length
            for (c = 0; c < (length * 2); c++)
                skip(buffer);
            break;

        default:
            return false;  // Type unsupported
    }

    return true;
}

bool MsgPack::search_key(const void* buffer, const MsgPack::RecID record_id) {
    uint8_t byte;
    uint16_t key;

    while (get_raw_byte(buffer, false, &byte)) {
        if (!get_u16(buffer, true, &key)) return false;  // Couldn't get key
        if (key == record_id) return true;               // Found record
        if (!skip(buffer)) return false;                 // Can't skip to next key
    };
    return false;
}

bool MsgPack::msgpack_get(const void* buffer, const size_t size, const RecID record_id, bool* value) {
    init_search(buffer, size);
    if (!search_key(buffer, record_id)) return false;   // Record not found
    if (!get_bool(buffer, false, value)) return false;  // Value isn't a bool

    return true;
}

bool MsgPack::msgpack_get(const void* buffer, const size_t size, const RecID record_id, uint8_t* value) {
    if (!init_search(buffer, size)) return false;
    if (!search_key(buffer, record_id)) return false;  // Record not found
    if (!get_u8(buffer, false, value)) return false;   // Value isn't a u8

    return true;
}

bool MsgPack::msgpack_get(const void* buffer, const size_t size, const RecID record_id, int64_t* value) {
    uint8_t byte;

    init_search(buffer, size);
    if (!search_key(buffer, record_id)) return false;  // Record not found

    if ((seek_ptr + 3) >= buffer_size) return false;                                      // End of buffer
    if ((get_raw_byte(buffer, true, &byte)) && (byte != MSGPACK_TYPE_S64)) return false;  // Value isn't a s64
    *value = ((int64_t)((uint8_t*)buffer)[seek_ptr] << 56) | ((int64_t)((uint8_t*)buffer)[seek_ptr + 1] << 48) |
             ((int64_t)((uint8_t*)buffer)[seek_ptr + 2] << 40) | ((int64_t)((uint8_t*)buffer)[seek_ptr + 3] << 32) |
             (((uint8_t*)buffer)[seek_ptr + 4] << 24) | (((uint8_t*)buffer)[seek_ptr + 5] << 16) |
             (((uint8_t*)buffer)[seek_ptr + 6] << 8) | ((uint8_t*)buffer)[seek_ptr + 7];

    return true;
}

bool MsgPack::msgpack_get(const void* buffer, const size_t size, const RecID record_id, std::string& value) {
    init_search(buffer, size);
    if (!search_key(buffer, record_id)) return false;     // Record not found
    if (!get_string(buffer, false, value)) return false;  // Value isn't a char array

    return true;
}

void MsgPack::msgpack_init(const void* buffer, size_t* ptr) {
    ((uint8_t*)buffer)[0] = MSGPACK_TYPE_MAP16;
    ((uint8_t*)buffer)[1] = 0;
    ((uint8_t*)buffer)[2] = 0;

    *ptr = 3;
}

void MsgPack::add_key(const void* buffer, size_t* ptr, const RecID record_id) {
    uint16_t key;

    ((uint8_t*)buffer)[(*ptr)++] = MSGPACK_TYPE_U16;
    ((uint8_t*)buffer)[(*ptr)++] = record_id >> 8;
    ((uint8_t*)buffer)[(*ptr)++] = record_id & 0xFF;

    // Auto-inc MAP16 size which should be at the beginning of the buffer

    key = (((uint8_t*)buffer)[1] << 8) | ((uint8_t*)buffer)[2];
    key++;

    ((uint8_t*)buffer)[1] = key >> 8;
    ((uint8_t*)buffer)[2] = key & 0xFF;
}

void MsgPack::msgpack_add(const void* buffer, size_t* ptr, const RecID record_id, bool value) {
    add_key(buffer, ptr, record_id);

    if (value)
        ((uint8_t*)buffer)[(*ptr)++] = MSGPACK_TRUE;
    else
        ((uint8_t*)buffer)[(*ptr)++] = MSGPACK_FALSE;
}

void MsgPack::msgpack_add(const void* buffer, size_t* ptr, const RecID record_id, uint8_t value) {
    add_key(buffer, ptr, record_id);

    if (value < 128) {
        ((uint8_t*)buffer)[(*ptr)++] = value;
    } else {
        ((uint8_t*)buffer)[(*ptr)++] = MSGPACK_TYPE_U8;
        ((uint8_t*)buffer)[(*ptr)++] = value;
    }
}

void MsgPack::msgpack_add(const void* buffer, size_t* ptr, const RecID record_id, int64_t value) {
    uint8_t c;

    add_key(buffer, ptr, record_id);

    ((uint8_t*)buffer)[(*ptr)++] = MSGPACK_TYPE_S64;

    for (c = 0; c < 8; c++)
        ((uint8_t*)buffer)[(*ptr)++] = (value >> (8 * (7 - c))) & 0xFF;
}

bool MsgPack::msgpack_add(const void* buffer, size_t* ptr, const RecID record_id, std::string value) {
    uint8_t c;
    size_t length;

    add_key(buffer, ptr, record_id);

    length = value.size();

    if (length < 32) {
        ((uint8_t*)buffer)[(*ptr)++] = length | 0xA0;  // Fixstr
    } else if ((length >= 32) && (length < 256)) {
        ((uint8_t*)buffer)[(*ptr)++] = MSGPACK_TYPE_STR8;
        ((uint8_t*)buffer)[(*ptr)++] = length;
    } else if ((length >= 256) && (length < 65536)) {
        ((uint8_t*)buffer)[(*ptr)++] = MSGPACK_TYPE_STR16;
        ((uint8_t*)buffer)[(*ptr)++] = length >> 8;
        ((uint8_t*)buffer)[(*ptr)++] = length & 0xFF;
    } else {
        return false;
    }

    for (c = 0; c < length; c++)
        ((uint8_t*)buffer)[(*ptr)++] = value[c];

    return true;
}