jitterbug/wire.c

#include <string.h>
#include <stdio.h>
#include "try.h"
#include "wire.h"

bool wire_align(wire_context_t *c, uint32_t alignment)
{
    if ((c->byte_cursor % alignment) == 0) {
        return true;
    }
    uint32_t new_cursor = c->byte_cursor + alignment - (c->byte_cursor % alignment);
    if (new_cursor >= c->data_len || new_cursor < c->byte_cursor) {
        return false;
    }
    c->byte_cursor = new_cursor;
    return true;
}

bool wire_write_align(wire_context_t *c, uint32_t alignment)
{
    if ((c->byte_cursor % alignment) == 0) {
        return true;
    }
    uint32_t new_cursor = c->byte_cursor + alignment - (c->byte_cursor % alignment);
    if (new_cursor >= c->data_len || new_cursor < c->byte_cursor) {
        return false;
    }
    while (c->byte_cursor < new_cursor) {
        c->data[c->byte_cursor] = '\0';
        c->byte_cursor++;
    }
    return true;
}

// SPEC: https://dbus.freedesktop.org/doc/dbus-specification.html#id-1.4.6
_WIRE_DEF_BYTE_TYPE(i8, int8_t);
_WIRE_DEF_BYTE_TYPE(u8, uint8_t);
_WIRE_DEF_TYPE(i16, int16_t, 2);
_WIRE_DEF_TYPE(u16, uint16_t, 2);
_WIRE_DEF_TYPE(i32, int32_t, 4);
_WIRE_DEF_TYPE(u32, uint32_t, 4);
_WIRE_DEF_TYPE(boolean, uint32_t, 4);

char *wire_get_string(wire_context_t *c, uint32_t *out_len)
{
    uint32_t len = *TRY_NONNULL(uint32_t*, wire_get_u32(c), NULL);
    if (out_len) {
        *out_len = len;
    }

    uint32_t new_cursor = c->byte_cursor + len + 1; /* + 1 because of the extra null termination that strings have */
    if (new_cursor >= c->data_len) {
        return NULL;
    }

    char *str = (char *)&c->data[c->byte_cursor];

    c->byte_cursor = new_cursor; 

    return str;
}

char *wire_set_string(wire_context_t *c, const char *str)
{
    uint32_t len = strlen(str);
    TRY_NONNULL(uint32_t*, wire_set_u32(c, len), NULL);

    uint32_t new_cursor = c->byte_cursor + len + 1; /* + 1 because of the extra null termination that strings have */
    if (new_cursor >= c->data_len) {
        return NULL;
    }

    // TODO?
    char *dest = (char*)&c->data[c->byte_cursor];
    memcpy(dest, str, len + 1);

    c->byte_cursor = new_cursor; 

    return dest;
}

char *wire_get_signature(wire_context_t *c, uint8_t *out_len)
{
    uint8_t len = *TRY_NONNULL(uint8_t*, wire_get_u8(c), NULL);
    if (out_len) {
        *out_len = len;
    }

    uint32_t new_cursor = c->byte_cursor + len + 1; /* + 1 because of the extra null termination that strings have */
    if (new_cursor >= c->data_len) {
        return NULL;
    }

    char *str = (char *)&c->data[c->byte_cursor];

    c->byte_cursor = new_cursor; 

    return str;
}

char *wire_set_signature(wire_context_t *c, const char *str)
{
    uint8_t len = strlen(str);
    TRY_NONNULL(uint8_t*, wire_set_u8(c, len), NULL);

    uint32_t new_cursor = c->byte_cursor + len + 1; /* + 1 because of the extra null termination that strings have */
    if (new_cursor >= c->data_len) {
        return NULL;
    }

    // TODO?
    char *dest = (char*)&c->data[c->byte_cursor];
    memcpy(dest, str, len + 1);

    c->byte_cursor = new_cursor; 

    return dest;
}

int wire_parse_message(wire_context_t *c, wire_message_t *msg)
{
    // SPEC: https://dbus.freedesktop.org/doc/dbus-specification.html#message-protocol-messages
    msg->endianness = *TRY_NONNULL(uint8_t*, wire_get_u8(c), -1);
    msg->type = *TRY_NONNULL(uint8_t*, wire_get_u8(c), -1);
    msg->flags = *TRY_NONNULL(uint8_t*, wire_get_u8(c), -1);
    msg->protocol_version = *TRY_NONNULL(uint8_t*, wire_get_u8(c), -1);
    msg->body_length = *TRY_NONNULL(uint32_t*, wire_get_u32(c), -1);
    msg->serial = *TRY_NONNULL(uint32_t*, wire_get_u32(c), -1);
    msg->header_fields_length = *TRY_NONNULL(uint32_t*, wire_get_u32(c), -1);

    size_t header_fields_end = c->byte_cursor + msg->header_fields_length;

    printf(" > start parse!\n");

    while (c->byte_cursor < header_fields_end) {
        uint8_t field_code = *TRY_NONNULL(uint8_t*, wire_get_u8(c), -1);
        TRY_NONNULL(char*, wire_get_signature(c, NULL), -1);

        switch (field_code) {
            case DBUS_HEADER_FIELD_PATH: /* through */
            case DBUS_HEADER_FIELD_INTERFACE: /* through */
            case DBUS_HEADER_FIELD_MEMBER: /* through */
            case DBUS_HEADER_FIELD_DESTINATION: {
                char *str = TRY_NONNULL(char*, wire_get_string(c, NULL), -1);
                printf("field: %s\n", str);
                msg->fields[field_code].t.str = str;
            } break;
            case DBUS_HEADER_FIELD_SIGNATURE: {
                char *str = TRY_NONNULL(char*, wire_get_signature(c, NULL), -1);
                printf("field: %s\n", str);
                msg->fields[field_code].t.str = str;
            } break;
            case DBUS_HEADER_FIELD_REPLY_SERIAL: {
                uint32_t u = *TRY_NONNULL(uint32_t*, wire_get_u32(c), -1);
                printf("field: %d\n", u);
                msg->fields[field_code].t.u32 = u;
            } break;
            default: {
                printf("header: unknown field: %d\n", field_code);
                return -1;
            } break;
        }
        msg->fields[field_code].present = true;

        // Structs are always aligned to `8`.
        // SPEC: https://dbus.freedesktop.org/doc/dbus-specification.html#id-1.4.6
        TRY_NONNULL(bool, wire_align(c, 8), -1);

        if (msg->header_fields_count++ > WIRE_MESSAGE_MAX_HEADER_FIELDS) {
            return -1;
        }
    }

    printf(" > parsed!!\n");

    /* header ends at 8 byte boundry */
    TRY_NONNULL(bool, wire_align(c, 8), -1);

    return 0;
}

int wire_compose_reply(wire_context_t *c, wire_message_t *msg, const char *signature, uint32_t **out_body_length)
{
    
    TRY_NONNULL(uint8_t*, wire_set_u8(c, msg->endianness), -1); /* endianness */
    TRY_NONNULL(uint8_t*, wire_set_u8(c, DBUS_MESSAGE_METHOD_RETURN), -1); /* type */
    TRY_NONNULL(uint8_t*, wire_set_u8(c, 0), -1); /* flags */
    TRY_NONNULL(uint8_t*, wire_set_u8(c, DBUS_PROTOCOL_VERSION), -1); /* protocol_version */
    uint32_t *body_length = TRY_NONNULL(uint32_t*, wire_set_u32(c, 0), -1); /* body length */
    TRY_NONNULL(uint32_t*, wire_set_u32(c, msg->serial+1), -1); /* serial */
    uint32_t *header_fields_length = TRY_NONNULL(uint32_t*, wire_set_u32(c, 0), -1); /* header_fields_length */

    uint32_t header_fields_start = c->byte_cursor;

    /* header fields */
    {
        /* SIGNATURE */
        {
            /* byte (field code) */
            TRY_NONNULL(uint8_t*, wire_set_u8(c, DBUS_HEADER_FIELD_SIGNATURE), -1);
            /* variant */
            /* signature */
            TRY_NONNULL(char*, wire_set_signature(c, "g"), -1);
            /* signature */
            TRY_NONNULL(char*, wire_set_signature(c, signature), -1);

            // need to align to 8 byte boundry after each array element?
            TRY_NONNULL(bool, wire_write_align(c, 8), -1);
        }

        /* REPLY_SERIAL */
        {
            /* byte (field code) */
            TRY_NONNULL(uint8_t*, wire_set_u8(c, DBUS_HEADER_FIELD_REPLY_SERIAL), -1);
            /* variant */
            /* signature */
            TRY_NONNULL(char*, wire_set_signature(c, "u"), -1);
            /* reply serial */
            TRY_NONNULL(uint32_t*, wire_set_u32(c, msg->serial), -1);
        }
    }

    *header_fields_length = c->byte_cursor - header_fields_start;

    // header ends on an 8 byte alignment
    TRY_NONNULL(bool, wire_write_align(c, 8), -1);

    if (out_body_length) {
        *out_body_length = body_length;
    }

    return 0;
}

int wire_compose_error(wire_context_t *c, wire_message_t *msg, const char *error_name)
{
    
    TRY_NONNULL(uint8_t*, wire_set_u8(c, msg->endianness), -1); /* endianness */
    TRY_NONNULL(uint8_t*, wire_set_u8(c, DBUS_MESSAGE_ERROR), -1); /* type */
    TRY_NONNULL(uint8_t*, wire_set_u8(c, 0), -1); /* flags */
    TRY_NONNULL(uint8_t*, wire_set_u8(c, DBUS_PROTOCOL_VERSION), -1); /* protocol_version */
    TRY_NONNULL(uint32_t*, wire_set_u32(c, 0), -1); /* body length */
    TRY_NONNULL(uint32_t*, wire_set_u32(c, msg->serial+1), -1); /* serial */
    uint32_t *header_fields_length = TRY_NONNULL(uint32_t*, wire_set_u32(c, 0), -1); /* header_fields_length */

    uint32_t header_fields_start = c->byte_cursor;

    /* header fields */
    {
        /* ERROR_NAME */
        {
            /* byte (field code) */
            TRY_NONNULL(uint8_t*, wire_set_u8(c, DBUS_HEADER_FIELD_ERROR_NAME), -1);
            /* variant */
            /* signature */
            TRY_NONNULL(char*, wire_set_signature(c, "s"), -1);
            /* string */
            TRY_NONNULL(char*, wire_set_string(c, error_name), -1);

            // need to align to 8 byte boundry after each array element?
            TRY_NONNULL(bool, wire_write_align(c, 8), -1);
        }

        /* REPLY_SERIAL */
        {
            /* byte (field code) */
            TRY_NONNULL(uint8_t*, wire_set_u8(c, DBUS_HEADER_FIELD_REPLY_SERIAL), -1);
            /* variant */
            /* signature */
            TRY_NONNULL(char*, wire_set_signature(c, "u"), -1);
            /* reply serial */
            TRY_NONNULL(uint32_t*, wire_set_u32(c, msg->serial), -1);
        }
    }

    *header_fields_length = c->byte_cursor - header_fields_start;

    // header ends on an 8 byte alignment
    TRY_NONNULL(bool, wire_write_align(c, 8), -1);

    return 0;
}

int wire_compose_unicast_reply(wire_context_t *c, wire_context_t *msg_c, wire_message_t *msg, char *sender_unique_name)
{
    
    TRY_NONNULL(uint8_t*, wire_set_u8(c, msg->endianness), -1); /* endianness */
    TRY_NONNULL(uint8_t*, wire_set_u8(c, msg->type), -1); /* type */
    TRY_NONNULL(uint8_t*, wire_set_u8(c, 0), -1); /* flags */
    TRY_NONNULL(uint8_t*, wire_set_u8(c, DBUS_PROTOCOL_VERSION), -1); /* protocol_version */
    TRY_NONNULL(uint32_t*, wire_set_u32(c, msg->body_length), -1); /* body length */
    TRY_NONNULL(uint32_t*, wire_set_u32(c, msg->serial), -1); /* serial */
    uint32_t *header_fields_length = TRY_NONNULL(uint32_t*, wire_set_u32(c, 0), -1); /* header_fields_length */

    uint32_t header_fields_start = c->byte_cursor;

    switch (msg->type) {
        case DBUS_MESSAGE_METHOD_CALL: {
            // these fields are required for DBUS_MESSAGE_METHOD_CALL, so we must (hackily) check for their presence
            // interface is not strictly required by the spec, however we require it here anyways
            wire_message_field_t *path_field = &msg->fields[DBUS_HEADER_FIELD_PATH];
            wire_message_field_t *interface_field = &msg->fields[DBUS_HEADER_FIELD_INTERFACE];
            wire_message_field_t *member_field = &msg->fields[DBUS_HEADER_FIELD_MEMBER];
            wire_message_field_t *signature_field = &msg->fields[DBUS_HEADER_FIELD_SIGNATURE];
            if (!path_field->present || !interface_field->present || !interface_field->present) {
                return -1;
            }

            TRY_NONNULL(uint8_t*, wire_set_u8(c, DBUS_HEADER_FIELD_PATH), -1);
            TRY_NONNULL(char*, wire_set_signature(c, "o"), -1);
            TRY_NONNULL(char*, wire_set_string(c, path_field->t.str), -1);
            TRY_NONNULL(bool, wire_write_align(c, 8), -1);

            TRY_NONNULL(uint8_t*, wire_set_u8(c, DBUS_HEADER_FIELD_INTERFACE), -1);
            TRY_NONNULL(char*, wire_set_signature(c, "s"), -1);
            TRY_NONNULL(char*, wire_set_string(c, interface_field->t.str), -1);
            TRY_NONNULL(bool, wire_write_align(c, 8), -1);

            TRY_NONNULL(uint8_t*, wire_set_u8(c, DBUS_HEADER_FIELD_MEMBER), -1);
            TRY_NONNULL(char*, wire_set_signature(c, "s"), -1);
            TRY_NONNULL(char*, wire_set_string(c, member_field->t.str), -1);
            TRY_NONNULL(bool, wire_write_align(c, 8), -1);

            if (signature_field->present) {
                TRY_NONNULL(uint8_t*, wire_set_u8(c, DBUS_HEADER_FIELD_SIGNATURE), -1);
                TRY_NONNULL(char*, wire_set_signature(c, "g"), -1);
                TRY_NONNULL(char*, wire_set_signature(c, signature_field->t.str), -1);
                TRY_NONNULL(bool, wire_write_align(c, 8), -1);
            }

            TRY_NONNULL(uint8_t*, wire_set_u8(c, DBUS_HEADER_FIELD_SENDER), -1);
            TRY_NONNULL(char*, wire_set_signature(c, "s"), -1);
            TRY_NONNULL(char*, wire_set_string(c, sender_unique_name), -1);
            // last element, don't align
        } break;

        case DBUS_MESSAGE_METHOD_RETURN: {
            wire_message_field_t *reply_serial_field = &msg->fields[DBUS_HEADER_FIELD_REPLY_SERIAL];
            if (!reply_serial_field->present) {
                return -1;
            }

            TRY_NONNULL(uint8_t*, wire_set_u8(c, DBUS_HEADER_FIELD_REPLY_SERIAL), -1);
            TRY_NONNULL(char*, wire_set_signature(c, "u"), -1);
            TRY_NONNULL(uint32_t*, wire_set_u32(c, reply_serial_field->t.u32), -1);
            TRY_NONNULL(bool, wire_write_align(c, 8), -1);

            wire_message_field_t *signature_field = &msg->fields[DBUS_HEADER_FIELD_SIGNATURE];
            if (signature_field->present) {
                TRY_NONNULL(uint8_t*, wire_set_u8(c, DBUS_HEADER_FIELD_SIGNATURE), -1);
                TRY_NONNULL(char*, wire_set_signature(c, "g"), -1);
                TRY_NONNULL(char*, wire_set_string(c, signature_field->t.str), -1);
                TRY_NONNULL(bool, wire_write_align(c, 8), -1);
            }

            TRY_NONNULL(uint8_t*, wire_set_u8(c, DBUS_HEADER_FIELD_SENDER), -1);
            TRY_NONNULL(char*, wire_set_signature(c, "s"), -1);
            TRY_NONNULL(char*, wire_set_string(c, sender_unique_name), -1);
            // last element, don't align
        } break;

        default: {
            return -1;
        } break;
    }

    *header_fields_length = c->byte_cursor - header_fields_start;

    // header ends on an 8 byte alignment
    TRY_NONNULL(bool, wire_write_align(c, 8), -1);

    if (c->byte_cursor + msg->body_length > c->data_len) {
        return -1;
    }

    memcpy(&c->data[c->byte_cursor], &msg_c->data[msg_c->byte_cursor], msg->body_length);
    c->byte_cursor += msg->body_length;

    return 0;
}