path: root/src/lib/du_buff.c

/*
 * Ouroboros - Copyright (C) 2016
 *
 * Data Unit Buffer
 *
 *    Dimitri Staessens <dimitri.staessens@intec.ugent.be>
 *    Sander Vrijders   <sander.vrijders@intec.ugent.be>
 *
 * 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 of the License, 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; if not, write to the Free Software
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

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

#include <ouroboros/du_buff.h>
#include <ouroboros/list.h>

#define OUROBOROS_PREFIX "du_buff"

#include "ouroboros/logs.h"

#define DU_BLOCK_DATA_SIZE (DU_BUFF_BLOCK_SIZE - sizeof (struct buffer))

struct buffer {
        uint8_t        * data;
        size_t           size;
        struct list_head list;
};

struct du_buff {
        struct buffer  * buffer;
        size_t           size;
        size_t           du_head;
        size_t           du_tail;
};

static void buffer_destroy(struct buffer * buf)
{
        if (buf == NULL) {
                LOG_DBGF("Bogus input, bugging out.");
                return;
        }

        free (buf->data);
        free (buf);
}

static void buffer_destroy_list(struct buffer * head)
{
        struct list_head * ptr;
        struct list_head * n;

        if (head == NULL) {
                LOG_DBGF("Bogus input, bugging out.");
                return;
        }

        list_for_each_safe(ptr, n, &(head->list)) {
                struct buffer * tmp = list_entry(ptr, struct buffer, list);
                list_del(ptr);
                buffer_destroy(tmp);
        }
        free(head);
}

static struct buffer * buffer_create (size_t size, size_t headspace, size_t len)
{
        struct buffer * head = NULL;
        size_t          remaining = size;
        size_t          ts = size - (headspace + len);

        if (headspace > DU_BLOCK_DATA_SIZE || ts > DU_BLOCK_DATA_SIZE) {
                LOG_ERR("Illegal du_buff: Cannot fit PCI in DU_BUFF_BLOCK.");
                return NULL;
        }

        head = malloc(sizeof *head);
        if (head == NULL)
                return NULL;

        head->size = 0;
        head->data = NULL;

        INIT_LIST_HEAD(&(head->list));

        while (remaining > 0) {
                struct buffer * buf;

                size_t sz;

                if (size > DU_BLOCK_DATA_SIZE
                           && remaining - ts <=  DU_BLOCK_DATA_SIZE
                           && remaining != ts) {
                        sz = remaining - ts;
                } else if (size > DU_BLOCK_DATA_SIZE && remaining == ts) {
                        sz = ts;
                } else {
                        sz = remaining < DU_BLOCK_DATA_SIZE ?
                                remaining : DU_BLOCK_DATA_SIZE;
                }

                buf = malloc(sizeof *buf);
                if (buf == NULL) {
                        LOG_WARN("Could not allocate struct.");
                        free(head);
                        return NULL;
                }

                if (sz > 0) {
                        buf->data = malloc(sz);
                        if (buf->data == NULL) {
                                LOG_WARN("Could not allocate memory block.");
                                buffer_destroy_list(head);
                                free(head);
                                free(buf);
                                return NULL;
                        }
                } else {
                        buf->data = NULL;
                }

                buf->size = sz;

                list_add_tail(&(buf->list), &(head->list));

                remaining -= buf->size;
        }

        return head;
}

static struct buffer * buffer_seek(const struct buffer * head, size_t pos)
{
        struct list_head * ptr = NULL;
        size_t cur_buf_start   = 0;
        size_t cur_buf_end     = 0;

        if (head == NULL) {
                LOG_DBGF("Bogus input, bugging out.");
                return NULL;
        }

        list_for_each(ptr, &(head->list)) {
                struct buffer * tmp = list_entry(ptr, struct buffer, list);

                cur_buf_end = cur_buf_start + tmp->size;
                if (cur_buf_end > pos)
                        return tmp;

                cur_buf_start = cur_buf_end;
        }

        return NULL;
}

static uint8_t * buffer_seek_pos(const struct buffer * head, size_t pos)
{
        struct list_head * ptr = NULL;
        size_t cur_buf_start   = 0;
        size_t cur_buf_end     = 0;

        if (head == NULL) {
                LOG_DBGF("Bogus input, bugging out.");
                return NULL;
        }

        list_for_each(ptr, &(head->list)) {
                struct buffer * tmp = list_entry(ptr, struct buffer, list);

                cur_buf_end = cur_buf_start + tmp->size;

                if (cur_buf_end > pos)
                        return tmp->data + (pos - cur_buf_start);

                cur_buf_start = cur_buf_end;
        }

        return NULL;
}

static int buffer_copy_data(struct buffer * head,
                     size_t          pos,
                     const void    * src,
                     size_t          len)
{
        struct list_head * ptr       = NULL;
        struct buffer    * buf_start = NULL;
        struct buffer    * buf_end   = NULL;
        uint8_t          * ptr_start = NULL;
        size_t             space_in_buf;
        size_t             bytes_remaining;
        uint8_t          * copy_pos  = NULL;

        if (head == NULL || src == NULL) {
                LOG_DBGF("Bogus input, bugging out.");
                return -EINVAL;
        }

        if (len == 0) {
                LOG_DBGF("Nothing to copy.");
                return 0;
        }

        buf_start = buffer_seek(head, pos);
        buf_end   = buffer_seek(head, pos + len - 1);

        if (buf_start == NULL || buf_end == NULL) {
                LOG_DBGF("Index out of bounds %lu, %lu", pos, pos + len);
                return -EINVAL;
        }

        ptr_start = buffer_seek_pos(head, pos);

        if (buf_start == buf_end) {
                memcpy(ptr_start, src, len);
                return 0;
        }

        copy_pos = (uint8_t *)src;
        bytes_remaining = len;
        list_for_each(ptr, &(head->list)) {
                struct buffer * tmp = list_entry(ptr, struct buffer, list);
                if (tmp != buf_start)
                        continue;

                space_in_buf = (tmp->data + tmp->size) - ptr_start;
                if (space_in_buf >= bytes_remaining) {
                        memcpy(ptr_start, copy_pos, bytes_remaining);
                        return 0;
                }
                else
                        memcpy(ptr_start, copy_pos, space_in_buf);
                bytes_remaining -= space_in_buf;
                copy_pos += space_in_buf;
        }

        return 0;
}

du_buff_t * du_buff_create(size_t size)
{
        du_buff_t * dub = malloc(sizeof *dub);

        if (dub == NULL) {
                LOG_DBGF("Bogus input, bugging out.");
                return NULL;
        }

        dub->buffer  = NULL;
        dub->size    = size;
        dub->du_head = 0;
        dub->du_tail = 0;

        return dub;
}

void du_buff_destroy(du_buff_t * dub)
{
        if (dub == NULL) {
                LOG_DBGF("Bogus input, bugging out.");
                return;
        }
        buffer_destroy_list(dub->buffer);
        free (dub);
}

int du_buff_init(du_buff_t * dub,
                 size_t      start,
                 uint8_t   * data,
                 size_t      len)
{
        if (dub == NULL || data == NULL) {
                LOG_DBGF("Bogus input, bugging out.");
                return -EINVAL;
        }

        if (start >= dub->size) {
                LOG_DBGF("Index out of bounds %lu.", start);
                return -EINVAL;
        }

        if (start + len > dub->size) {
                LOG_DBGF("Buffer too small for data.");
                return -EINVAL;
        }

        dub->buffer = buffer_create(dub->size, start, len);
        if (dub->buffer == NULL)
                return -ENOMEM;

        dub->du_head = start;
        dub->du_tail = start + len;

        return buffer_copy_data(dub->buffer, start, data, len);
}

uint8_t * du_buff_head_alloc(du_buff_t * dub, size_t size)
{
        if (dub == NULL) {
                LOG_DBGF("Bogus input, bugging out.");
                return NULL;
        }

        if ((long) (dub->du_head - size) < 0) {
                LOG_WARN("Failed to allocate PCI headspace.");
                return NULL;
        }

        dub->du_head -= size;

        return (buffer_seek_pos(dub->buffer, dub->du_head));
}

uint8_t * du_buff_tail_alloc(du_buff_t * dub, size_t size)
{
        if (dub == NULL) {
                LOG_DBGF("Bogus input, bugging out.");
                return NULL;
        }

        if (dub->du_tail + size >= dub->size) {
                LOG_WARN("Failed to allocate PCI tailspace.");
                return NULL;
        }

        dub->du_tail += size;

        return (buffer_seek_pos(dub->buffer, dub->du_tail));
}

int du_buff_head_release(du_buff_t * dub, size_t size)
{
        if (dub == NULL) {
                LOG_DBGF("Bogus input, bugging out.");
                return -EINVAL;
        }

        if (size > dub->du_tail - dub->du_head) {
                LOG_WARN("Tried to release beyond sdu boundary.");
                return -EOVERFLOW;
        }

        dub->du_head += size;

        /* FIXME: copy some random crap to the buffer for security */

        return 0;
}

int du_buff_tail_release(du_buff_t * dub, size_t size)
{
        if (dub == NULL) {
                LOG_DBGF("Bogus input, bugging out.");
                return -EINVAL;
        }

        if (size > dub->du_tail - dub->du_head) {
                LOG_WARN("Tried to release beyond sdu boundary.");
                return -EOVERFLOW;
        }

        dub->du_tail -= size;

        /* FIXME: copy some random crap to the buffer for security */

        return 0;
}