Stack.h

fir2.c

@@ -0,0 +1,69 @@
+#ifndef FIR_FILTER_H
+#define FIR_FILTER_H
+
+#include <stdint.h>
+#include <string.h>
+
+/* 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;
+}

stack.c

@@ -0,0 +1,166 @@
+#ifndef STACK_H
+#define STACK_H
+
+#include <assert.h>
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#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 */
+    }
+}