diff --git a/libs/stfu/stfu.c b/libs/stfu/stfu.c new file mode 100644 index 0000000000..326fabb985 --- /dev/null +++ b/libs/stfu/stfu.c @@ -0,0 +1,256 @@ +/* + * STFU (S)ort (T)ransportable (F)ramed (U)tterances + * Copyright (c) 2007 Anthony Minessale II + * + * Permission is hereby granted, free of charge, to any person + * obtaining a copy of this software and associated documentation + * files (the "Software"), to deal in the Software without + * restriction, including without limitation the rights to use, + * copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the + * Software is furnished to do so, subject to the following + * conditions: + * + * The above copyright notice and this permission notice shall be + * included in all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, + * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES + * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND + * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT + * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, + * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR + * OTHER DEALINGS IN THE SOFTWARE. + * + * THOSE WHO DISAGREE MAY CERTIANLY STFU + */ +#include "stfu.h" + +struct stfu_queue { + struct stfu_frame *array; + struct stfu_frame int_frame; + uint32_t array_size; + uint32_t array_len; + uint32_t wr_len; + uint32_t last_index; +}; +typedef struct stfu_queue stfu_queue_t; + +struct stfu_instance { + struct stfu_queue a_queue; + struct stfu_queue b_queue; + struct stfu_queue *in_queue; + struct stfu_queue *out_queue; + uint32_t last_ts; + uint32_t interval; + uint32_t miss_count; + uint8_t running; +}; + + +static void stfu_n_init_aqueue(stfu_queue_t *queue, uint32_t qlen) +{ + queue->array = calloc(qlen, sizeof(struct stfu_frame)); + assert(queue->array != NULL); + memset(queue->array, 0, sizeof(struct stfu_frame) * qlen); + queue->array_size = qlen; + queue->int_frame.plc = 1; +} + +void stfu_n_destroy(stfu_instance_t **i) +{ + stfu_instance_t *ii; + + if (i && *i) { + ii = *i; + *i = NULL; + free(ii->a_queue.array); + free(ii->b_queue.array); + free(ii); + } +} + +stfu_instance_t *stfu_n_init(uint32_t qlen) +{ + struct stfu_instance *i; + + i = malloc(sizeof(*i)); + assert(i != NULL); + memset(i, 0, sizeof(*i)); + stfu_n_init_aqueue(&i->a_queue, qlen); + stfu_n_init_aqueue(&i->b_queue, qlen); + i->in_queue = &i->a_queue; + i->out_queue = &i->b_queue; + return i; +} + +static int32_t stfu_n_measure_interval(stfu_instance_t *i, stfu_queue_t *queue) +{ + uint32_t index; + int32_t d, most = 0, last = 0, this, track[STFU_MAX_TRACK] = {0}; + + for(index = 0; index < queue->array_len; index++) { + this = queue->array[index].ts; + if (last) { + + if ((d = this - last) > 0 && d / 10 < STFU_MAX_TRACK) { + track[(d/10)]++; + } + } + + last = this; + } + + for(index = 0; index < STFU_MAX_TRACK; index++) { + if (track[index] > most) { + most = index; + } + } + + return most * 10; + +} + +static int16_t stfu_n_process(stfu_instance_t *i, stfu_queue_t *queue) +{ + if (!i->interval && !(i->interval = stfu_n_measure_interval(i, queue))) { + return -1; + } + + return 0; +} + +stfu_status_t stfu_n_add_data(stfu_instance_t *i, uint32_t ts, void *data, size_t datalen, int last) +{ + uint32_t index; + stfu_frame_t *frame; + size_t cplen = 0; + + + if (last || i->in_queue->array_len == i->in_queue->array_size) { + stfu_queue_t *other_queue; + + if (i->out_queue->wr_len < i->out_queue->array_len) { + return STFU_IT_FAILED; + } + + other_queue = i->in_queue; + i->in_queue = i->out_queue; + i->out_queue = other_queue; + + i->in_queue->array_len = 0; + i->out_queue->wr_len = 0; + i->out_queue->last_index = 0; + i->miss_count = 0; + + if (stfu_n_process(i, i->out_queue) < 0) { + return STFU_IT_FAILED; + } + for(index = 0; index < i->out_queue->array_len; index++) { + i->out_queue->array[index].was_read = 0; + } + } + + if (last) { + return STFU_IM_DONE; + } + + index = i->in_queue->array_len++; + frame = &i->in_queue->array[index]; + + if ((cplen = datalen) > sizeof(frame->data)) { + cplen = sizeof(frame->data); + } + + + memcpy(frame->data, data, cplen); + frame->ts = ts; + frame->dlen = cplen; + frame->was_read = 0; + + return STFU_IT_WORKED; +} + +stfu_frame_t *stfu_n_read_a_frame(stfu_instance_t *i) +{ + uint32_t index, index2; + uint32_t should_have = 0; + stfu_frame_t *frame = NULL, *rframe = NULL; + + + if ((i->out_queue->wr_len == i->out_queue->array_len) || !i->out_queue->array_len) { + return NULL; + } + + if (i->running) { + should_have = i->last_ts + i->interval; + } + + for(index = 0; index < i->out_queue->array_len; index++) { + if (i->out_queue->array[index].was_read) { + continue; + } + + frame = &i->out_queue->array[index]; + + if (frame->ts != should_have) { + int tried = 0; + for (index2 = 0; index2 < i->out_queue->array_len; index2++) { + if (i->out_queue->array[index2].was_read) { + continue; + } + tried++; + if (i->out_queue->array[index2].ts == should_have) { + rframe = &i->out_queue->array[index2]; + i->out_queue->last_index = index2; + goto done; + } + } + for (index2 = 0; index2 < i->in_queue->array_len; index2++) { + if (i->in_queue->array[index2].was_read) { + continue; + } + tried++; + if (i->in_queue->array[index2].ts == should_have) { + rframe = &i->in_queue->array[index2]; + goto done; + } + } + + i->miss_count++; + + if (i->miss_count > 10) { + i->out_queue->wr_len = i->out_queue->array_len; + i->last_ts = should_have = frame->ts; + return NULL; + } + i->last_ts = should_have; + rframe = &i->out_queue->int_frame; + rframe->dlen = i->out_queue->array[i->out_queue->last_index].dlen; + /* poor man's plc.. Copy the last frame, but we flag it so you can use a better one if you wish */ + memcpy(rframe->data, i->out_queue->array[i->out_queue->last_index].data, rframe->dlen); + rframe->ts = should_have; + i->out_queue->wr_len++; + i->running = 1; + return rframe; + } else { + rframe = &i->out_queue->array[index]; + i->out_queue->last_index = index; + goto done; + } + } + + done: + + if (rframe) { + i->out_queue->wr_len++; + i->last_ts = rframe->ts; + rframe->was_read = 1; + i->running = 1; + } + + return rframe; +} + + diff --git a/libs/stfu/stfu.h b/libs/stfu/stfu.h new file mode 100644 index 0000000000..743b2b47b2 --- /dev/null +++ b/libs/stfu/stfu.h @@ -0,0 +1,96 @@ +/* + * STFU (S)ort (T)ransportable (F)ramed (U)tterances + * Copyright (c) 2007 Anthony Minessale II + * + * Permission is hereby granted, free of charge, to any person + * obtaining a copy of this software and associated documentation + * files (the "Software"), to deal in the Software without + * restriction, including without limitation the rights to use, + * copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the + * Software is furnished to do so, subject to the following + * conditions: + * + * The above copyright notice and this permission notice shall be + * included in all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, + * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES + * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND + * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT + * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, + * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING + * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR + * OTHER DEALINGS IN THE SOFTWARE. + * + * THOSE WHO DISAGREE MAY CERTIANLY STFU + */ + +#ifndef STFU_H +#define STFU_H +#ifdef __cplusplus +extern "C" { +#endif +#ifdef __STUPIDFORMATBUG__ +} +#endif + +#include + +#ifdef _MSC_VER +#ifndef uint32_t +typedef unsigned __int8 uint8_t; +typedef unsigned __int16 uint16_t; +typedef unsigned __int32 uint32_t; +typedef unsigned __int64 uint64_t; +typedef __int8 int8_t; +typedef __int16 int16_t; +typedef __int32 int32_t; +typedef __int64 int64_t; +typedef unsigned long in_addr_t; +#endif +#define snprintf _snprintf +#else +#include +#include +#include +#include +#include +#include +#endif +#include + +#define STFU_DATALEN 16384 +#define STFU_QLEN 300 +#define STFU_MAX_TRACK 256 + +typedef enum { + STFU_IT_FAILED, + STFU_IT_WORKED, + STFU_IM_DONE +} stfu_status_t; + +struct stfu_frame { + uint32_t ts; + uint8_t data[STFU_DATALEN]; + size_t dlen; + uint8_t was_read; + uint8_t plc; +}; +typedef struct stfu_frame stfu_frame_t; + +struct stfu_instance; +typedef struct stfu_instance stfu_instance_t; + +void stfu_n_destroy(stfu_instance_t **i); +stfu_instance_t *stfu_n_init(uint32_t qlen); +stfu_status_t stfu_n_add_data(stfu_instance_t *i, uint32_t ts, void *data, size_t datalen, int last); +stfu_frame_t *stfu_n_read_a_frame(stfu_instance_t *i); +#define stfu_im_done() stfu_n_add_data(i, 0, NULL, 0, 1) +#define stfu_n_eat(i,t,d,l) stfu_n_add_data(i, t, d, l, 0) + +#ifdef __cplusplus +} +#endif +#endif /*STFU_H*/ +