diff --git a/lib/filters.c b/lib/filters.c index ab7e5bb..52333ce 100644 --- a/lib/filters.c +++ b/lib/filters.c @@ -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++) {