Compare commits
No commits in common. "4f436c9f15e4d5841bdeb0c8d0a93eb7ba783836" and "a7dfe216fb57eedc78c6c439fbf23c7f93380feb" have entirely different histories.
4f436c9f15
...
a7dfe216fb
2 changed files with 0 additions and 235 deletions
69
fir2.c
69
fir2.c
|
|
@ -1,69 +0,0 @@
|
||||||
#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;
|
|
||||||
}
|
|
||||||
166
stack.c
166
stack.c
|
|
@ -1,166 +0,0 @@
|
||||||
#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 */
|
|
||||||
}
|
|
||||||
}
|
|
||||||
Loading…
Add table
Add a link
Reference in a new issue