radio95/fm95
0
1
Fork 0
mirror of https://github.com/radio95-rnt/fm95.git synced 2026-02-26 19:23:51 +01:00
Code Issues Activity

this

Browse Source
This commit is contained in:
KubaPro010
2025-03-27 21:19:33 +01:00
parent 7594f352c1
commit 4121657da9
3 changed files with 22 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

43
lib/filters.c
View File

@@ -33,39 +33,30 @@ void init_lpf(LPFFilter *filter, float cutoff, int sample_rate) {
float process_lpf(LPFFilter *filter, float x) {
#if USE_NEON
float32x4_t x_vec = vdupq_n_f32(x);
float32x4_t y_vec = x_vec;
float32_t input = x;
float32x4_t *A_vec = (float32x4_t*)filter->A;
float32x4_t *d1_vec = (float32x4_t*)filter->d1;
float32x4_t *d2_vec = (float32x4_t*)filter->d2;
float32x4_t *w1_vec = (float32x4_t*)filter->w1;
float32x4_t *w2_vec = (float32x4_t*)filter->w2;
float32x4_t y_vec = vdupq_n_f32(input);
for (int i = 0; i < LPF_ORDER; i += 4) {
float32x4_t w1_term = vmulq_f32(*d1_vec, *w1_vec);
float32x4_t w2_term = vmulq_f32(*d2_vec, *w2_vec);
float32x4_t w0_new = vaddq_f32(vaddq_f32(w1_term, w2_term), y_vec);
for (int i = 0; i < LPF_ORDER; i++) {
float32x4_t d1_vec = vdupq_n_f32(filter->d1[i]);
float32x4_t d2_vec = vdupq_n_f32(filter->d2[i]);
float32x4_t w1_vec = vdupq_n_f32(filter->w1[i]);
float32x4_t w2_vec = vdupq_n_f32(filter->w2[i]);
float32x4_t A_vec = vdupq_n_f32(filter->A[i]);
float32x4_t two_w1 = vmulq_n_f32(*w1_vec, 2.0f);
y_vec = vmulq_f32(*A_vec, vaddq_f32(vaddq_f32(w0_new, two_w1), *w2_vec));
float32x4_t w1_term = vmulq_f32(d1_vec, w1_vec);
float32x4_t w2_term = vmulq_f32(d2_vec, w2_vec);
float32x4_t w0_new_vec = vaddq_f32(vaddq_f32(w1_term, w2_term), y_vec);
*w2_vec = *w1_vec;
*w1_vec = w0_new;
float32x4_t two_w1 = vmulq_n_f32(w1_vec, 2.0f);
float32x4_t output_term = vaddq_f32(w0_new_vec, vaddq_f32(two_w1, w2_vec));
y_vec = vmulq_f32(A_vec, output_term);
A_vec++;
d1_vec++;
d2_vec++;
w1_vec++;
w2_vec++;
filter->w2[i] = filter->w1[i];
filter->w1[i] = vgetq_lane_f32(w0_new_vec, 0);
}
float32x2_t y_low = vget_low_f32(y_vec);
float32x2_t y_high = vget_high_f32(y_vec);
float32x2_t y_sum = vadd_f32(y_low, y_high);
y_sum = vpadd_f32(y_sum, y_sum);
return vget_lane_f32(y_sum, 0);
return vgetq_lane_f32(y_vec, 0);
#else
float y = x;
for (int i = 0; i < LPF_ORDER; i++) {

14
lib/oscillator.c
View File

@@ -35,22 +35,8 @@ float get_oscillator_cos_multiplier_ni(Oscillator *osc, float multiplier) {
}
void advance_oscillator(Oscillator *osc) {
#if USE_NEON // Use NEON if available
float32x4_t v_phase = vdupq_n_f32(osc->phase);
float32x4_t v_increment = vdupq_n_f32(osc->phase_increment);
float32x4_t v_twopi = vdupq_n_f32(M_2PI);
v_phase = vaddq_f32(v_phase, v_increment);
uint32x4_t v_mask = vcgeq_f32(v_phase, v_twopi); // Check if phase >= 2π
float32x4_t v_wrapped = vsubq_f32(v_phase, v_twopi);
v_phase = vbslq_f32(v_mask, v_wrapped, v_phase);
osc->phase = vgetq_lane_f32(v_phase, 0);
#else // Scalar fallback if NEON is not available
osc->phase += osc->phase_increment;
if (osc->phase >= M_2PI) {
osc->phase -= M_2PI;
}
#endif
}

1
lib/oscillator.h
View File

@@ -2,7 +2,6 @@
#include "constants.h"
#include <math.h>
#include "optimization.h"
typedef struct {
float phase;