buffers.c 4.97 KB
Newer Older
Cedric Roux's avatar
 
Cedric Roux committed
1 2 3 4 5
#include <pthread.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>

6 7 8
#include <glib.h>

#include "rc.h"
Cedric Roux's avatar
 
Cedric Roux committed
9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54
#include "buffers.h"

extern int debug_buffers;

static
int buffer_fetch(buffer_t *buffer, uint32_t offset, int size, void *value);

/* Try to fetch 8 bits unsigned from the buffer */
uint8_t buffer_get_uint8_t(buffer_t *buffer, uint32_t offset)
{
    uint8_t value;

    buffer_fetch(buffer, offset, 1, &value);

    return value;
}

/* Try to fetch 16 bits unsigned from the buffer */
uint16_t buffer_get_uint16_t(buffer_t *buffer, uint32_t offset)
{
    uint16_t value;

    buffer_fetch(buffer, offset, 2, &value);

    return value;
}

/* Try to fetch 32 bits unsigned from the buffer */
uint32_t buffer_get_uint32_t(buffer_t *buffer, uint32_t offset)
{
    uint32_t value;

    buffer_fetch(buffer, offset, 4, &value);

    return value;
}

static
int buffer_fetch(buffer_t *buffer, uint32_t offset, int size, void *value)
{
    if (buffer == NULL || value == NULL)
        return -1;
    if (size <= 0)
        return -1;

    if (buffer->size_bytes < ((offset >> 3) + size)) {
55
        g_warning("Not enough data to fetch");
Cedric Roux's avatar
 
Cedric Roux committed
56 57 58 59 60 61 62 63 64
        return -1;
    }

    memcpy(value, &buffer->data[offset >> 3], size);
    buffer->buffer_current = &buffer->data[(offset >> 3) + size];

    return 0;
}

65 66 67 68 69 70 71 72 73 74 75
int buffer_fetch_nbytes(buffer_t *buffer, uint32_t offset, int n_bytes, uint8_t *value)
{
    if (buffer->size_bytes < ((offset >> 3) + n_bytes)) {
        g_warning("Not enough data to fetch");
        return -1;
    }
    memcpy(&value[0], &buffer->data[offset >> 3], n_bytes);

    return 0;
}

Cedric Roux's avatar
 
Cedric Roux committed
76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196
int buffer_fetch_bits(buffer_t *buffer, uint32_t offset, int nbits, uint32_t *value)
{
    uint32_t temp = 0;
    int i;

    if (buffer == NULL || value == NULL)
        return RC_BAD_PARAM;

    /* We cannot fetch more than 32 bits */
    if (nbits > 32)
        return RC_BAD_PARAM;

    for (i = 0; i < nbits; i++)
    {
        temp |= ((buffer->data[(offset + i) / 8] >> ((offset + i) % 8)) & 1) << i;
    }

    *value = temp;

    return RC_OK;
}

/**
 * @brief Create a new buffer from data
 * @param buffer caller reference where created buffer will be stored
 * @param data Data to attach to the new buffer
 * @param length Length of data buffer
 * @param data_static flag that indicates if data pointer has been statically (= 0) or not (!= 0)
 */
int buffer_new_from_data(buffer_t **buffer, uint8_t *data, const uint32_t length,
                         int data_static)
{
    buffer_t *new;

    if (!buffer)
        return RC_BAD_PARAM;

    new = malloc(sizeof(buffer_t));

    new->size_bytes = length;
    if (data && length > 0) {
        if (data_static == 0) {
            new->data = malloc(sizeof(uint8_t) * new->size_bytes);
            memcpy(new->data, data, new->size_bytes);
        } else {
            new->data = data;
        }
        new->buffer_current = &new->data[0];
    } else {
        new->buffer_current = NULL;
    }

    *buffer = new;

    return 0;
}

#define INDENTED(fILE, x, y)            \
do {                                    \
    int indentation = x;                \
    while(indentation--) fprintf(fILE, " ");  \
    y;                                  \
} while(0)

#define INDENT_BREAK    20

void buffer_dump(buffer_t *buffer, FILE *to)
{
    FILE *file = to;
    uint32_t i;

    if (!buffer)
        return;
    if (!to)
        to = stdout;

    fprintf(file, "<Buffer>\n");
    INDENTED(file, 4, fprintf(file, "<Length>%u<Length>\n", buffer->size_bytes));
    INDENTED(file, 4, fprintf(file, "<Bytes>\n"));
    for (i = 0; i < buffer->size_bytes; i++)
    {
        if ((i % INDENT_BREAK) == 0)
            fprintf(file, "        ");
        fprintf(file, "0x%02x ", buffer->data[i]);
        if ((i % INDENT_BREAK) == (INDENT_BREAK - 1))
            fprintf(file, "\n");
    }
    if ((i % INDENT_BREAK) != (INDENT_BREAK - 1))
        fprintf(file, "\n");
    INDENTED(file, 4, fprintf(file, "</Bytes>\n"));
    fprintf(file, "</Buffer>\n");
}

int buffer_append_data(buffer_t *buffer, const uint8_t *data, const uint32_t length)
{
    if (!buffer)
        return -1;

    if (data && length > 0) {
        if (!buffer->data) {
            buffer->size_bytes = length;
            buffer->data = malloc(sizeof(uint8_t) * buffer->size_bytes);
            memcpy(buffer->data, data, buffer->size_bytes);
        } else {
            buffer->data = realloc(buffer->data, sizeof(uint8_t) * (buffer->size_bytes + length));
            memcpy(&buffer->data[buffer->size_bytes], data, length);
            buffer->size_bytes += length;
        }
        buffer->buffer_current = &buffer->data[0];
    }

    return 0;
}

int buffer_has_enouch_data(buffer_t *buffer, uint32_t offset, uint32_t to_get)
{
    int underflow;
    if (!buffer)
        return -1;
    underflow = (buffer->size_bytes >= ((offset + to_get) / 8)) ? 0 : -1;
    if (underflow && debug_buffers)
197 198
        g_debug("Detected Underflow offset %u, to_get %u, buffer size %u\n",
                offset, to_get, buffer->size_bytes);
Cedric Roux's avatar
 
Cedric Roux committed
199 200
    return underflow;
}