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

try another filter neon implemntaiton

Browse Source
This commit is contained in:
KubaPro010
2025-03-27 21:17:45 +01:00
parent a54d7f9895
commit 7594f352c1
1 changed files with 31 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

45
lib/filters.c
View File

@@ -33,22 +33,39 @@ void init_lpf(LPFFilter *filter, float cutoff, int sample_rate) {
float process_lpf(LPFFilter *filter, float x) {
#if USE_NEON
float32x4_t y_vec = vdupq_n_f32(x);
float32x4_t x_vec = vdupq_n_f32(x);
float32x4_t y_vec = x_vec;
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;
for (int i = 0; i < LPF_ORDER; i += 4) {
float32x4_t d1_vec = vld1q_f32(&filter->d1[i]);
float32x4_t d2_vec = vld1q_f32(&filter->d2[i]);
float32x4_t w1_vec = vld1q_f32(&filter->w1[i]);
float32x4_t w2_vec = vld1q_f32(&filter->w2[i]);
float32x4_t A_vec = vld1q_f32(&filter->A[i]);
float32x4_t w0_new = vmlaq_f32(y_vec, d1_vec, w1_vec);
w0_new = vmlaq_f32(w0_new, d2_vec, w2_vec);
float32x4_t temp = vmlaq_f32(w0_new, w1_vec, vdupq_n_f32(2.0f));
temp = vaddq_f32(temp, w2_vec);
y_vec = vmulq_f32(A_vec, temp);
vst1q_f32(&filter->w2[i], w1_vec);
vst1q_f32(&filter->w1[i], w0_new);
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);
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));
*w2_vec = *w1_vec;
*w1_vec = w0_new;
A_vec++;
d1_vec++;
d2_vec++;
w1_vec++;
w2_vec++;
}
return vgetq_lane_f32(y_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);
#else
float y = x;
for (int i = 0; i < LPF_ORDER; i++) {