diff --git a/fir2.c b/fir2.c new file mode 100644 index 0000000..4c1b277 --- /dev/null +++ b/fir2.c @@ -0,0 +1,69 @@ +#ifndef FIR_FILTER_H +#define FIR_FILTER_H + +#include +#include + +/* This needs to be a power of two for the bitmask optimizations to work */ +/* If a non power of two length is desired, change to modulo */ +#define FIR_LEN (1u << 5) +_Static_assert((FIR_LEN & (FIR_LEN - 1u)) == 0, "FIR_LEN must be a power of two"); + +typedef struct { + uint_fast8_t idx; + float buf[FIR_LEN]; + const float *coeffs; +} FIRFilter; + +void FIRFilter_init(FIRFilter *fir, const float *coef_array); +float FIRFilter_update(FIRFilter *fir, float inp); + +#endif // FIR_FILTER_H + +// #include "fir_filter.h" + +void FIRFilter_init(FIRFilter *fir, const float *coeffs) { + memset(fir, 0, sizeof(FIRFilter)); + fir->coeffs = coeffs; +} + +float FIRFilter_update(FIRFilter *fir, float inp) { + float out = 0.f; + fir->buf[fir->idx] = inp; + fir->idx = (fir->idx + 1) & (FIR_LEN - 1); + + // Compute the latest output sample by applying a convolution + uint8_t sum_idx = fir->idx; + +#ifndef CONFIG_FIR_UNROLL + + for (size_t i = 0; i < FIR_LEN; i++) { + sum_idx = (sum_idx - 1) & (FIR_LEN - 1); + out += fir->coeffs[i] * fir->buf[sum_idx]; + } + +#else // CONFIG_FIR_UNROLL + + int count = FIR_LEN / 8; + int leftover = FIR_LEN % 8; + const float *c = fir->coeffs; + + switch (leftover) { + case 0: + do { + sum_idx = (sum_idx - 1) & (FIR_LEN - 1); + out += *c++ * fir->buf[sum_idx]; + case 7: sum_idx = (sum_idx - 1) & (FIR_LEN - 1); out += *c++ * fir->buf[sum_idx]; + case 6: sum_idx = (sum_idx - 1) & (FIR_LEN - 1); out += *c++ * fir->buf[sum_idx]; + case 5: sum_idx = (sum_idx - 1) & (FIR_LEN - 1); out += *c++ * fir->buf[sum_idx]; + case 4: sum_idx = (sum_idx - 1) & (FIR_LEN - 1); out += *c++ * fir->buf[sum_idx]; + case 3: sum_idx = (sum_idx - 1) & (FIR_LEN - 1); out += *c++ * fir->buf[sum_idx]; + case 2: sum_idx = (sum_idx - 1) & (FIR_LEN - 1); out += *c++ * fir->buf[sum_idx]; + case 1: sum_idx = (sum_idx - 1) & (FIR_LEN - 1); out += *c++ * fir->buf[sum_idx]; + } while (--count > 0); + } + +#endif // CONFIG_FIR_UNROLL + + return out; +} diff --git a/stack.c b/stack.c new file mode 100644 index 0000000..9ce82e0 --- /dev/null +++ b/stack.c @@ -0,0 +1,166 @@ +#ifndef STACK_H +#define STACK_H + +#include +#include +#include +#include +#include +#include +#include + +#if defined(__GNUC__) || defined(__clang__) +#define MUST_CHECK __attribute__((warn_unused_result)) +#else +#define MUST_CHECK +#endif + +typedef struct { + uint8_t *buf; //!< Pointer to the internal buffer */ + size_t buf_sz; //!< The length of the allocated buffer, in bytes */ + size_t read_ptr; //!< Indicates where to read/write */ + size_t item_sz; //!< The size of the contained item, in bytes */ +} Stack; + +typedef enum { + Ok = 0, //!< Everything is fine + Error = -1, //!< Something went wrong + StackFull = 1, //!< The stack is full + StackEmpty = 2, //!< The stack is empty +} StackResult; + +MUST_CHECK StackResult stack_init(Stack *stack, size_t capacity, size_t item_sz); +MUST_CHECK StackResult stack_init_raw(Stack *stack, uint8_t *buf, size_t buf_sz, size_t item_sz); +MUST_CHECK StackResult stack_destroy(Stack *stack); +MUST_CHECK StackResult stack_push(Stack *stack, const void *value); +MUST_CHECK StackResult stack_pop(Stack *stack, void *dest); + +static inline size_t stack_length(Stack *stack) { + return (stack->read_ptr / stack->item_sz); +} + +static inline size_t stack_capacity(Stack *stack) { + return (stack->buf_sz / stack->item_sz); +} + +static inline bool stack_full(Stack *stack) { + return (stack->buf_sz == stack->read_ptr); +} + +static inline bool stack_empty(Stack *stack) { + return (stack->read_ptr < stack->item_sz); +} + +#endif // STACK_H + +// #include "stack.h" + +StackResult stack_init(Stack *stack, size_t capacity, size_t item_sz) { + return stack_init_raw(stack, malloc(capacity * item_sz), capacity * item_sz, item_sz); +} + +StackResult stack_init_raw(Stack *stack, uint8_t *buf, size_t buf_sz, size_t item_sz) { + if (!memset(stack, 0, sizeof(Stack))) { + return Error; + } + + stack->buf = buf; + if (!stack->buf) { + return Error; + } + + stack->buf_sz = buf_sz; + stack->item_sz = item_sz; + stack->read_ptr = 0; + + memset(stack->buf, 0, buf_sz); + + return Ok; +} + +StackResult stack_push(Stack *stack, const void *value) { + if (stack->read_ptr + stack->item_sz > stack->buf_sz) + return StackFull; + + memcpy((char *)stack->buf + stack->read_ptr, value, stack->item_sz); + stack->read_ptr += stack->item_sz; + + return Ok; +} + +StackResult stack_pop(Stack *stack, void *dest) { + if (stack->read_ptr < stack->item_sz) + return StackEmpty; + + stack->read_ptr -= stack->item_sz; + memcpy(dest, (char *)stack->buf + stack->read_ptr, stack->item_sz); + + return Ok; +} + +StackResult stack_destroy(Stack *stack) { + free(stack->buf); + + stack->buf = NULL; + stack->buf_sz = 0; + stack->item_sz = 0; + stack->read_ptr = 0; + + return Ok; +} + +int main(void) { + Stack s; + if (stack_init(&s, 128, sizeof(int)) != Ok) + return 1; + + assert(stack_capacity(&s) == 128); + + int r; // Just used for testing + + { /* Sanity check: push some data and check the output */ + for (int i = 1; i <= 10; i++) { + if (stack_push(&s, &i) != Ok) + return 1; + } + + while (stack_pop(&s, &r) == Ok) { + printf("Number %d\n", r); + } + } + + /* Should tell us StackEmpty */ + assert(stack_pop(&s, &r) == StackEmpty); + + /* Make sure it tolerates filling and behaves expectedly */ + while (stack_push(&s, &r) == Ok); + assert(stack_full(&s)); + assert(stack_length(&s) == 128); + assert(stack_push(&s, &r) == StackFull); + + /* Drain it and check for proper behaviour */ + while (stack_pop(&s, &r) == Ok); + assert(stack_pop(&s, &r) == StackEmpty); + assert(stack_empty(&s)); + assert(stack_length(&s) == 0); + + if (stack_destroy(&s) != Ok) { + printf("Error: stack_destroy did not destroy properly..."); + return 1; + } + + { + char buf[32]; + Stack s2; + if (stack_init_raw(&s2, (uint8_t *)buf, 32, sizeof(int)) != Ok) { + printf("Error: stack_init_raw did not init properly..."); + return 1; + } + + assert(stack_capacity(&s2) == 8); + assert(stack_length(&s2) == 0); + assert(stack_empty(&s2)); + + /* Make sure to not call stack_destroy or free here, since buf[] is static */ + } +}