skipstone/src/message.c

#include <stdlib.h>
#include <string.h>

#include <skipstone/link.h>
#include <skipstone/map.h>
#include <skipstone/queue.h>
#include <skipstone/message.h>

enum part_type {
    SKIPSTONE_MESSAGE_PART_STRING,
    SKIPSTONE_MESSAGE_PART_UINT8,
    SKIPSTONE_MESSAGE_PART_UINT16
};

struct part {
    enum part_type type;
    enum skipstone_message_endianness endianness;
    uint8_t size;
};

struct _skipstone_message {
    uint16_t size, id;
    skipstone_queue *parts;
};

struct endpoint {
    skipstone_message_handler *handler;
    void *context;
    uint16_t id;
};

struct _skipstone_message_service {
    skipstone_map *endpoints;
    skipstone_queue *pending;
};

skipstone_message *skipstone_message_new(uint16_t id) {
    skipstone_message *message;

    if ((message = malloc(sizeof(*message))) == NULL) {
        goto error_malloc_message;
    }

    if ((message->parts = skipstone_queue_new()) == NULL) {
        goto error_queue_new_parts;
    }

    message->size = 0;
    message->id   = id;

    return message;

error_queue_new_parts:
    free(message);

error_malloc_message:
    return NULL;
}

static int append_part(skipstone_message *message,
        void *buf, uint8_t size, enum part_type type) {
    struct part *part;

    if ((part = malloc(sizeof(*part) + size)) == NULL) {
        goto error_malloc_part;
    }

    part->type       = type;
    part->endianness = SKIPSTONE_MESSAGE_ENDIAN_LITTLE;
    part->size       = size;

    memcpy(part + 1, buf, size);

    if (skipstone_queue_add(message->parts, part) < 0) {
        goto error_queue_add_parts;
    }

    return 0;

error_queue_add_parts:
    free(part);

error_malloc_part:
    return -1;
}

int skipstone_message_append_string(skipstone_message *message,
        char *string, uint8_t size) {
    return append_part(message, string, size, SKIPSTONE_MESSAGE_PART_STRING);
}

int skipstone_message_append_uint8(skipstone_message *message, uint8_t value) {
    return append_part(message, &value, sizeof(value),
        SKIPSTONE_MESSAGE_PART_UINT8);
}

int skipstone_message_append_uint16(skipstone_message *message, uint16_t value) {
    return append_part(message, &value, sizeof(value),
        SKIPSTONE_MESSAGE_PART_UINT16);
}

int skipstone_message_pack(skipstone_message *message,
        void *buf, uint16_t *size) {
    size_t offset = 0;

    struct part *part;

    while (skipstone_queue_remove(message->parts, (void **)&part)) {
        switch (part->type) {
            case SKIPSTONE_MESSAGE_PART_STRING: {
                ((uint8_t *)buf)[offset] = part->size;

                memcpy((uint8_t *)buf + offset + sizeof(uint8_t), part + 1, part->size);

                offset += sizeof(uint8_t) + part->size;

                break;
            }

            case SKIPSTONE_MESSAGE_PART_UINT8: {
                ((uint8_t *)buf)[offset] = ((uint8_t *)(part + 1))[0];

                offset += sizeof(uint8_t);

                break;
            }

            case SKIPSTONE_MESSAGE_PART_UINT16: {
                ((uint16_t *)((uint8_t *)buf + offset))[0] = ((uint16_t *)(part + 1))[0];

                offset += sizeof(uint16_t);

                break;
            }
        }
    }

    *size = offset;

    return 0;
}

skipstone_message_service *skipstone_message_service_new() {
    skipstone_message_service *service;

    if ((service = malloc(sizeof(*service))) == NULL) {
        goto error_malloc_service;
    }

    if ((service->endpoints = skipstone_map_new()) == NULL) {
        goto error_map_new_endpoints;
    }

    if ((service->pending = skipstone_queue_new()) == NULL) {
        goto error_queue_new_pending;
    }

    return service;

error_queue_new_pending:
    skipstone_map_destroy(service->endpoints);

error_map_new_endpoints:
    free(service);

error_malloc_service:
    return NULL;
}

int skipstone_message_service_register(skipstone_message_service *service,
        uint16_t id, skipstone_message_handler *handler, void *context) {
    struct endpoint *endpoint;

    if ((endpoint = malloc(sizeof(*endpoint))) == NULL) {
        goto error_malloc_endpoint;
    }

    endpoint->id      = id;
    endpoint->handler = handler;
    endpoint->context = context;

    return skipstone_map_set(service->endpoints, id, endpoint);

error_malloc_endpoint:
    return -1;
}

int skipstone_message_service_deregister(skipstone_message_service *service, uint16_t id) {
    return skipstone_map_set((skipstone_map *)service, id, NULL);
}

int skipstone_message_service_queue(skipstone_message_service *service,
        void *buf, uint16_t size, uint16_t id) {
    skipstone_message_header *message;

    if ((message = malloc(sizeof(*message) + size)) == NULL) {
        goto error_malloc_message;
    }

    message->size = size;
    message->id   = id;

    memcpy(message + 1, buf, size);

    if (skipstone_queue_add(service->pending, message) < 0) {
        goto error_queue_add_pending;
    }

    return 0;

error_queue_add_pending:
    free(message);

error_malloc_message:
    return -1;
}

int skipstone_message_service_run(skipstone_message_service *service,
        skipstone_link *link) {
    void *buf;

    if ((buf = malloc(SKIPSTONE_MESSAGE_MAX_PAYLOAD)) == NULL) {
        goto error_malloc_buf;
    }

    while (1) {
        skipstone_message_header *message;
        struct endpoint *endpoint;
        uint16_t size, id;

        if (skipstone_link_recv(link, buf, &size, &id) < 0) {
            goto error_io;
        }

        if ((endpoint = skipstone_map_get(service->endpoints, id)) != NULL) {
            if (endpoint->handler(service, buf, size, id, endpoint->context) < 0) {
                goto error_io;
            }
        }

        while (skipstone_queue_remove(service->pending, (void **)&message)) {
            if (skipstone_link_send(link, message + 1, message->size, message->id) < 0) {
                goto error_io;
            }
        }
    }

    return 0;

error_io:
    free(buf);

error_malloc_buf:
    return -1;
}