paddlec 0.0.1

data/ext/libpaddlec/fir_filter.c

@@ -0,0 +1,724 @@
+/* Copyright (C) 2019 Théotime Bollengier <theotime.bollengier@gmail.com>
+ *
+ * This file is part of PaddleC
+ * 
+ * PaddleC is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ * 
+ * PaddleC is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ * 
+ * You should have received a copy of the GNU General Public License
+ * along with PaddleC. If not, see <https://www.gnu.org/licenses/>.
+ */
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <math.h>
+#include "libpaddlec.h"
+
+
+pdlc_fir_filter_t* pdlc_fir_filter_new(int order)
+{
+	pdlc_fir_filter_t *fir;
+
+	fir = malloc(sizeof(pdlc_fir_filter_t));
+
+	fir->coefs = NULL;
+	fir->stater = NULL;
+	fir->statei = NULL;
+	fir->nb_coefs = 0;
+	fir->state_len = 0;
+	fir->coef_len = 0;
+	fir->index = 0;
+	fir->index_mask = 0;
+	fir->counter = 0;
+	fir->max_counter = 1;
+
+	if (order >= 0)
+		pdlc_fir_filter_initialize(fir, order);
+
+	return fir;
+}
+
+
+int pdlc_fir_filter_get_coef_at(const pdlc_fir_filter_t* fir, int index, float *value)
+{
+	if (index < 0 || index >= (int)fir->nb_coefs)
+		return -1;
+
+	if (value)
+		*value = fir->coefs[0][fir->nb_coefs - 1 - index];
+
+	return 0;
+}
+
+
+void pdlc_fir_filter_reset(pdlc_fir_filter_t* fir)
+{
+	memset(fir->stater, 0, fir->coef_len * sizeof(float));
+	memset(fir->statei, 0, fir->coef_len * sizeof(float));
+	fir->index = 0;
+	fir->counter = 0;
+}
+
+
+int pdlc_fir_filter_get_factor(const pdlc_fir_filter_t* fir)
+{
+	return fir->max_counter;
+}
+
+
+void pdlc_fir_filter_interpolator_initialize(pdlc_fir_filter_t* fir, int order, int factor)
+{
+	pdlc_fir_filter_initialize(fir, order);
+
+	if (factor < 1)
+		factor = 1;
+	if (factor > 32)
+		factor = 32;
+
+	fir->max_counter = factor;
+}
+
+
+void pdlc_fir_filter_decimator_initialize(pdlc_fir_filter_t* fir, int order, int factor)
+{
+	pdlc_fir_filter_initialize(fir, order);
+
+	if (factor < 1)
+		factor = 1;
+	if (factor > 32)
+		factor = 32;
+
+	fir->max_counter = factor;
+}
+
+
+#if defined __AVX__
+#include "fir_filter_avx.c"
+#elif defined __SSE__
+#include "fir_filter_sse.c"
+#elif (defined __ARM_NEON) && ((__ARM_NEON_FP & 4) == 4)
+#include "fir_filter_neon.c"
+#else
+
+
+void pdlc_fir_filter_inspect(pdlc_fir_filter_t* fir)
+{
+	size_t i;
+	printf("nb_coefs: %u, state_len: %u, coef_len: %u, index_mask: %x, index: %u\n",
+			fir->nb_coefs, fir->state_len, fir->coef_len, fir->index_mask, fir->index);
+	printf("state: [%.7g", fir->stater[0]);
+	for (i = 1; i < fir->state_len; i++)
+		printf(", %.7g", fir->stater[i]);
+	printf("]\ncoefs: [%.7g", fir->coefs[0][0]);
+	for (i = 1; i < fir->coef_len; i++)
+		printf(", %.7g", fir->coefs[0][i]);
+	printf("]\n");
+}
+
+
+void pdlc_fir_filter_initialize(pdlc_fir_filter_t* fir, int order)
+{
+	if (fir->coefs) {
+		if (fir->coefs[0])
+			free(fir->coefs[0]);
+		free(fir->coefs);
+		fir->coefs = NULL;
+	}
+
+	if (fir->stater)
+		free(fir->stater);
+	fir->stater = NULL;
+
+	if (fir->statei)
+		free(fir->statei);
+	fir->statei = NULL;
+
+	fir->nb_coefs = 0;
+	fir->state_len = 0;
+	fir->coef_len = 0;
+	fir->index = 0;
+	fir->index_mask = 0;
+	fir->counter = 0;
+	fir->max_counter = 1;
+
+	if (order < 0)
+		return;
+
+	if (order > 67108863) {
+		fprintf(stderr, "ERROR: libpaddlec: Filter order cannot be greater than 67108864\n");
+		exit(EXIT_FAILURE);
+	}
+
+	fir->nb_coefs = (unsigned int)(order + 1);
+	fir->state_len = (unsigned int)(pow(2.0, ceil(log2(fir->nb_coefs))));
+	fir->coef_len = fir->nb_coefs;
+	fir->index = 0;
+	fir->index_mask = fir->state_len - 1;
+
+	fir->coefs = malloc(1*sizeof(float*));
+	if (fir->coefs == NULL) {
+		fprintf(stderr, "ERROR: libpaddlec: Cannot allocate %lu bytes for FIR!\n", 1 * sizeof(float*));
+		exit(EXIT_FAILURE);
+	}
+
+	fir->coefs[0] = malloc(fir->coef_len * sizeof(float));
+	if (fir->coefs[0] == NULL) {
+		fprintf(stderr, "ERROR: libpaddlec: Cannot allocate %lu bytes for FIR!\n", fir->coef_len * sizeof(float));
+		exit(EXIT_FAILURE);
+	}
+
+	fir->stater = malloc(fir->state_len * sizeof(float));
+	if (fir->stater == NULL) {
+		fprintf(stderr, "ERROR: libpaddlec: Cannot allocate %lu bytes for FIR!\n", fir->state_len * sizeof(float));
+		exit(EXIT_FAILURE);
+	}
+
+	fir->statei = malloc(fir->state_len * sizeof(float));
+	if (fir->statei == NULL) {
+		fprintf(stderr, "ERROR: libpaddlec: Cannot allocate %lu bytes for FIR!\n", fir->state_len * sizeof(float));
+		exit(EXIT_FAILURE);
+	}
+
+	memset(fir->stater,   0, fir->state_len * sizeof(float));
+	memset(fir->statei,   0, fir->state_len * sizeof(float));
+	memset(fir->coefs[0], 0, fir->coef_len  * sizeof(float));
+}
+
+
+void pdlc_fir_filter_free(pdlc_fir_filter_t* fir)
+{
+	if (!fir)
+		return;
+
+	if (fir->coefs) {
+		if (fir->coefs[0])
+			free(fir->coefs[0]);
+		free(fir->coefs);
+	}
+
+	if (fir->stater)
+		free(fir->stater);
+
+	if (fir->statei)
+		free(fir->statei);
+
+	free(fir);
+}
+
+
+size_t pdlc_fir_filter_size(pdlc_fir_filter_t* fir)
+{
+	size_t res;
+
+	res  = sizeof(pdlc_fir_filter_t);
+	res += sizeof(float*);
+	res += sizeof(float) * fir->state_len * 2;
+	res += sizeof(float) * fir->coef_len * 1;
+
+	return res;
+}
+
+
+int pdlc_fir_filter_set_coef_at(pdlc_fir_filter_t* fir, int index, float  value)
+{
+	if (index < 0 || index >= (int)fir->nb_coefs)
+		return -1;
+
+	fir->coefs[0][fir->nb_coefs - 1 - index] = value;
+
+	return 0;
+}
+
+
+float pdlc_fir_filter_filter_float(pdlc_fir_filter_t* fir, float sample, float *delayed)
+{
+	const unsigned int nb_coefs = fir->nb_coefs;
+	const unsigned int flt_len  = fir->state_len;
+	const unsigned int mask = fir->index_mask;
+	const unsigned int start_index = (flt_len + fir->index + 1 - nb_coefs) & mask;
+	const unsigned int middle_index = (start_index + nb_coefs / 2) & mask;
+	const float *coefs = fir->coefs[0];
+	float acc = 0.0f;
+	unsigned int i, j;
+
+	fir->stater[fir->index] = sample;
+	//pdlc_fir_filter_inspect(fir);
+	fir->index = (fir->index + 1) & mask;
+
+	if (delayed) {
+		if (nb_coefs & 1)
+			*delayed = fir->stater[middle_index];
+		else
+			*delayed = (fir->stater[middle_index] + fir->stater[(middle_index - 1) & mask]) / 2.0f;
+	}
+
+	j = start_index;
+	for (i = 0; i < nb_coefs; i++) {
+		acc += coefs[i] * fir->stater[j];
+		j = (j+1) & mask;
+	}
+
+	return acc;
+}
+
+
+pdlc_complex_t pdlc_fir_filter_filter_complex(pdlc_fir_filter_t* fir, pdlc_complex_t sample, pdlc_complex_t *delayed)
+{
+	const unsigned int nb_coefs = fir->nb_coefs;
+	const unsigned int flt_len  = fir->state_len;
+	const unsigned int mask = fir->index_mask;
+	const unsigned int start_index = (flt_len + fir->index + 1 - nb_coefs) & mask;
+	const unsigned int middle_index = (start_index + nb_coefs / 2) & mask;
+	const float *coefs = fir->coefs[0];
+	pdlc_complex_t acc = {0.0f, 0.0f};
+	unsigned int i, j;
+
+	fir->stater[fir->index] = sample.real;
+	fir->statei[fir->index] = sample.imag;
+	fir->index = (fir->index + 1) & mask;
+
+	j = start_index;
+	for (i = 0; i < nb_coefs; i++) {
+		acc.real += coefs[i] * fir->stater[j];
+		acc.imag += coefs[i] * fir->statei[j];
+		j = (j+1) & mask;
+	}
+
+	if (delayed) {
+		if (nb_coefs & 1) {
+			delayed->real = fir->stater[middle_index];
+			delayed->imag = fir->statei[middle_index];
+		}
+		else {
+			delayed->real = (fir->stater[middle_index] + fir->stater[(middle_index - 1) & mask]) / 2.0f;
+			delayed->imag = (fir->statei[middle_index] + fir->statei[(middle_index - 1) & mask]) / 2.0f;
+		}
+	}
+
+	return acc;
+}
+
+
+pdlc_buffer_t* pdlc_fir_filter_filter_float_buffer(pdlc_fir_filter_t* fir, const pdlc_buffer_t *ifbuf, pdlc_buffer_t *ofbuf, pdlc_buffer_t *delayed)
+{
+	const unsigned int nb_coefs = fir->nb_coefs;
+	const unsigned int flt_len  = fir->state_len;
+	const unsigned int mask = fir->index_mask;
+	const float *coefs = fir->coefs[0];
+	unsigned int start_index = (flt_len + fir->index + 1 - nb_coefs) & mask;
+	unsigned int middle_index;
+	float acc;
+	unsigned int i, j;
+	size_t k;
+
+	if (!ofbuf)
+		ofbuf = pdlc_buffer_new(ifbuf->length);
+	else if (ofbuf->length != ifbuf->length)
+		pdlc_buffer_resize(ofbuf, ifbuf->length, 0);
+
+	if (delayed) {
+		if (delayed->length != ifbuf->length)
+			pdlc_buffer_resize(delayed, ifbuf->length, 0);
+		middle_index = (start_index + nb_coefs / 2) & mask;
+		if (nb_coefs & 1) {
+			for (k = 0; k < ifbuf->length; k++) {
+				fir->stater[fir->index] = ifbuf->data[k];
+				fir->index = (fir->index + 1) & mask;
+				acc = 0.0f;
+				j = start_index;
+				for (i = 0; i < nb_coefs; i++) {
+					acc += coefs[i] * fir->stater[j];
+					j = (j+1) & mask;
+				}
+				ofbuf->data[k] = acc;
+				start_index = (start_index + 1) & mask;
+				delayed->data[k] = fir->stater[middle_index];
+				middle_index = (middle_index + 1) & mask;
+			}
+		}
+		else {
+			for (k = 0; k < ifbuf->length; k++) {
+				fir->stater[fir->index] = ifbuf->data[k];
+				fir->index = (fir->index + 1) & mask;
+				acc = 0.0f;
+				j = start_index;
+				for (i = 0; i < nb_coefs; i++) {
+					acc += coefs[i] * fir->stater[j];
+					j = (j+1) & mask;
+				}
+				ofbuf->data[k] = acc;
+				start_index = (start_index + 1) & mask;
+				delayed->data[k] = (fir->stater[middle_index] + fir->stater[(middle_index - 1) & mask]) / 2.0f;
+				middle_index = (middle_index + 1) & mask;
+			}
+		}
+	}
+	else {
+		for (k = 0; k < ifbuf->length; k++) {
+			fir->stater[fir->index] = ifbuf->data[k];
+			fir->index = (fir->index + 1) & mask;
+			acc = 0.0f;
+			j = start_index;
+			for (i = 0; i < nb_coefs; i++) {
+				acc += coefs[i] * fir->stater[j];
+				j = (j+1) & mask;
+			}
+			ofbuf->data[k] = acc;
+			start_index = (start_index + 1) & mask;
+		}
+	}
+
+	return ofbuf;
+}
+
+
+pdlc_complex_buffer_t* pdlc_fir_filter_filter_complex_buffer(pdlc_fir_filter_t* fir, const pdlc_complex_buffer_t *icbuf, pdlc_complex_buffer_t *ocbuf, pdlc_complex_buffer_t *delayed)
+{
+	const unsigned int nb_coefs = fir->nb_coefs;
+	const unsigned int flt_len  = fir->state_len;
+	const unsigned int mask = fir->index_mask;
+	const float *coefs = fir->coefs[0];
+	unsigned int start_index = (flt_len + fir->index + 1 - nb_coefs) & mask;
+	unsigned int middle_index;
+	pdlc_complex_t acc;
+	unsigned int i, j;
+	size_t k;
+
+	if (!ocbuf)
+		ocbuf = pdlc_complex_buffer_new(icbuf->length);
+	else if (ocbuf->length != icbuf->length)
+		pdlc_complex_buffer_resize(ocbuf, icbuf->length, 0);
+
+	if (delayed) {
+		if (delayed->length != icbuf->length)
+			pdlc_complex_buffer_resize(delayed, icbuf->length, 0);
+		middle_index = (start_index + nb_coefs / 2) & mask;
+		if (nb_coefs & 1) {
+			for (k = 0; k < icbuf->length; k++) {
+				fir->stater[fir->index] = icbuf->data[k].real;
+				fir->statei[fir->index] = icbuf->data[k].imag;
+				fir->index = (fir->index + 1) & mask;
+				acc.real = 0.0f;
+				acc.imag = 0.0f;
+				j = start_index;
+				for (i = 0; i < nb_coefs; i++) {
+					acc.real += coefs[i] * fir->stater[j];
+					acc.imag += coefs[i] * fir->statei[j];
+					j = (j+1) & mask;
+				}
+				ocbuf->data[k] = acc;
+				start_index = (start_index + 1) & mask;
+				delayed->data[k].real = fir->stater[middle_index];
+				delayed->data[k].imag = fir->statei[middle_index];
+				middle_index = (middle_index + 1) & mask;
+			}
+		}
+		else {
+			for (k = 0; k < icbuf->length; k++) {
+				fir->stater[fir->index] = icbuf->data[k].real;
+				fir->statei[fir->index] = icbuf->data[k].imag;
+				fir->index = (fir->index + 1) & mask;
+				acc.real = 0.0f;
+				acc.imag = 0.0f;
+				j = start_index;
+				for (i = 0; i < nb_coefs; i++) {
+					acc.real += coefs[i] * fir->stater[j];
+					acc.imag += coefs[i] * fir->statei[j];
+					j = (j+1) & mask;
+				}
+				ocbuf->data[k] = acc;
+				start_index = (start_index + 1) & mask;
+				delayed->data[k].real = (fir->stater[middle_index] + fir->stater[(middle_index - 1) & mask]) / 2.0f;
+				delayed->data[k].imag = (fir->statei[middle_index] + fir->statei[(middle_index - 1) & mask]) / 2.0f;
+				middle_index = (middle_index + 1) & mask;
+			}
+		}
+	}
+	else {
+		for (k = 0; k < icbuf->length; k++) {
+			fir->stater[fir->index] = icbuf->data[k].real;
+			fir->statei[fir->index] = icbuf->data[k].imag;
+			fir->index = (fir->index + 1) & mask;
+			acc.real = 0.0f;
+			acc.imag = 0.0f;
+			j = start_index;
+			for (i = 0; i < nb_coefs; i++) {
+				acc.real += coefs[i] * fir->stater[j];
+				acc.imag += coefs[i] * fir->statei[j];
+				j = (j+1) & mask;
+			}
+			ocbuf->data[k] = acc;
+			start_index = (start_index + 1) & mask;
+		}
+	}
+
+	return ocbuf;
+}
+
+
+pdlc_buffer_t* pdlc_fir_filter_interpolate_float_buffer(pdlc_fir_filter_t* fir, const pdlc_buffer_t *ifbuf, pdlc_buffer_t *ofbuf)
+{
+	const unsigned int nb_coefs = fir->nb_coefs;
+	const unsigned int flt_len  = fir->state_len;
+	const unsigned int mask = fir->index_mask;
+	const float *coefs = fir->coefs[0];
+	const float ffactor = (float)(fir->max_counter);
+	const size_t ibuflen = ifbuf->length;
+	const size_t obuflen = ibuflen*fir->max_counter;
+	float *stater = fir->stater;
+	unsigned int start_index = (flt_len + fir->index + 1 - nb_coefs) & mask;
+	float acc;
+	unsigned int i, j;
+	size_t k, l;
+	int m;
+
+	if (!ofbuf)
+		ofbuf = pdlc_buffer_new(obuflen);
+	else if (ofbuf->length != obuflen)
+		pdlc_buffer_resize(ofbuf, obuflen, 0);
+
+	for (k = 0, l = 0; k < ibuflen; k++) {
+		stater[fir->index] = ifbuf->data[k];
+		fir->index = (fir->index + 1) & mask;
+		acc = 0.0f;
+		j = start_index;
+		for (i = 0; i < nb_coefs; i++) {
+			acc += coefs[i] * stater[j];
+			j = (j+1) & mask;
+		}
+		ofbuf->data[l++] = acc * ffactor;
+		start_index = (start_index + 1) & mask;
+
+		for (m = 1; m < fir->max_counter; m++) {
+			stater[fir->index] = 0.0f;
+			fir->index = (fir->index + 1) & mask;
+			acc = 0.0f;
+			j = start_index;
+			for (i = 0; i < nb_coefs; i++) {
+				acc += coefs[i] * stater[j];
+				j = (j+1) & mask;
+			}
+			ofbuf->data[l++] = acc * ffactor;
+			start_index = (start_index + 1) & mask;
+		}
+	}
+
+	return ofbuf;
+}
+
+
+pdlc_complex_buffer_t* pdlc_fir_filter_interpolate_complex_buffer(pdlc_fir_filter_t* fir, const pdlc_complex_buffer_t *icbuf, pdlc_complex_buffer_t *ocbuf)
+{
+	const unsigned int nb_coefs = fir->nb_coefs;
+	const unsigned int flt_len  = fir->state_len;
+	const unsigned int mask = fir->index_mask;
+	const float *coefs = fir->coefs[0];
+	const float ffactor = (float)(fir->max_counter);
+	const size_t ibuflen = icbuf->length;
+	const size_t obuflen = ibuflen*fir->max_counter;
+	float *stater = fir->stater;
+	float *statei = fir->statei;
+	unsigned int start_index = (flt_len + fir->index + 1 - nb_coefs) & mask;
+	float accr, acci;
+	unsigned int i, j;
+	size_t k, l;
+	int m;
+
+	if (!ocbuf)
+		ocbuf = pdlc_complex_buffer_new(obuflen);
+	else if (ocbuf->length != obuflen)
+		pdlc_complex_buffer_resize(ocbuf, obuflen, 0);
+
+	for (k = 0, l = 0; k < ibuflen; k++) {
+		stater[fir->index] = icbuf->data[k].real;
+		statei[fir->index] = icbuf->data[k].imag;
+		fir->index = (fir->index + 1) & mask;
+		accr = 0.0f;
+		acci = 0.0f;
+		j = start_index;
+		for (i = 0; i < nb_coefs; i++) {
+			accr += coefs[i] * stater[j];
+			acci += coefs[i] * statei[j];
+			j = (j+1) & mask;
+		}
+		ocbuf->data[l].real = accr * ffactor;
+		ocbuf->data[l++].imag = acci * ffactor;
+		start_index = (start_index + 1) & mask;
+
+		for (m = 1; m < fir->max_counter; m++) {
+			stater[fir->index] = 0.0f;
+			statei[fir->index] = 0.0f;
+			fir->index = (fir->index + 1) & mask;
+			accr = 0.0f;
+			acci = 0.0f;
+			j = start_index;
+			for (i = 0; i < nb_coefs; i++) {
+				accr += coefs[i] * stater[j];
+				acci += coefs[i] * statei[j];
+				j = (j+1) & mask;
+			}
+			ocbuf->data[l].real = accr * ffactor;
+			ocbuf->data[l++].imag = acci * ffactor;
+			start_index = (start_index + 1) & mask;
+		}
+	}
+
+	return ocbuf;
+}
+
+
+pdlc_buffer_t* pdlc_fir_filter_decimate_float_buffer(pdlc_fir_filter_t* fir, const pdlc_buffer_t *ifbuf, pdlc_buffer_t *ofbuf)
+{
+	const unsigned int nb_coefs = fir->nb_coefs;
+	const unsigned int flt_len  = fir->state_len;
+	const unsigned int mask = fir->index_mask;
+	const float *coefs = fir->coefs[0];
+	const size_t ibuflen = ifbuf->length;
+	const int mcounter = fir->max_counter;
+	const size_t obuflen = (size_t)ceil(((double)ibuflen - (double)((mcounter - fir->counter) % mcounter)) / (double)mcounter);
+	unsigned int start_index = (flt_len + fir->index + 1 - nb_coefs) & mask;
+	float *stater = fir->stater;
+	float acc;
+	unsigned int i, j;
+	size_t k, l;
+
+
+	if (!ofbuf)
+		ofbuf = pdlc_buffer_new(obuflen);
+	else if (ofbuf->length != obuflen)
+		pdlc_buffer_resize(ofbuf, obuflen, 0);
+
+
+	for (k = 0, l = 0; k < ibuflen; k++) {
+		stater[fir->index] = ifbuf->data[k];
+		fir->index = (fir->index + 1) & mask;
+		if (fir->counter == 0) {
+			acc = 0.0f;
+			j = start_index;
+			for (i = 0; i < nb_coefs; i++) {
+				acc += coefs[i] * stater[j];
+				j = (j+1) & mask;
+			}
+			ofbuf->data[l++] = acc;
+		}
+		start_index = (start_index + 1) & mask;
+		fir->counter = (fir->counter + 1) % mcounter;
+	}
+
+	return ofbuf;
+}
+
+
+pdlc_complex_buffer_t* pdlc_fir_filter_decimate_complex_buffer(pdlc_fir_filter_t* fir, const pdlc_complex_buffer_t *icbuf, pdlc_complex_buffer_t *ocbuf)
+{
+	const unsigned int nb_coefs = fir->nb_coefs;
+	const unsigned int flt_len  = fir->state_len;
+	const unsigned int mask = fir->index_mask;
+	const float *coefs = fir->coefs[0];
+	const size_t ibuflen = icbuf->length;
+	const int mcounter = fir->max_counter;
+	const size_t obuflen = (size_t)ceil(((double)ibuflen - (double)((mcounter - fir->counter) % mcounter)) / (double)mcounter);
+	unsigned int start_index = (flt_len + fir->index + 1 - nb_coefs) & mask;
+	float *stater = fir->stater;
+	float *statei = fir->statei;
+	float accr, acci;
+	unsigned int i, j;
+	size_t k, l;
+
+
+	if (!ocbuf)
+		ocbuf = pdlc_complex_buffer_new(obuflen);
+	else if (ocbuf->length != obuflen)
+		pdlc_complex_buffer_resize(ocbuf, obuflen, 0);
+
+
+	for (k = 0, l = 0; k < ibuflen; k++) {
+		stater[fir->index] = icbuf->data[k].real;
+		statei[fir->index] = icbuf->data[k].imag;
+		fir->index = (fir->index + 1) & mask;
+		if (fir->counter == 0) {
+			accr = 0.0f;
+			acci = 0.0f;
+			j = start_index;
+			for (i = 0; i < nb_coefs; i++) {
+				accr += coefs[i] * stater[j];
+				acci += coefs[i] * statei[j];
+				j = (j+1) & mask;
+			}
+			ocbuf->data[l].real = accr;
+			ocbuf->data[l].imag = acci;
+			l++;
+		}
+		start_index = (start_index + 1) & mask;
+		fir->counter = (fir->counter + 1) % mcounter;
+	}
+
+	return ocbuf;
+}
+
+
+pdlc_complex_buffer_t* pdlc_fir_filter_transform(pdlc_fir_filter_t* fir, const pdlc_buffer_t *ifbuf, pdlc_complex_buffer_t *ocbuf)
+{
+	const unsigned int nb_coefs = fir->nb_coefs;
+	const unsigned int flt_len  = fir->state_len;
+	const unsigned int mask = fir->index_mask;
+	const float *coefs = fir->coefs[0];
+	unsigned int start_index = (flt_len + fir->index + 1 - nb_coefs) & mask;
+	unsigned int middle_index = (start_index + nb_coefs / 2) & mask;
+	float acc;
+	unsigned int i, j;
+	size_t k;
+
+	if (!ocbuf)
+		ocbuf = pdlc_complex_buffer_new(ifbuf->length);
+	else if (ocbuf->length != ifbuf->length)
+		pdlc_complex_buffer_resize(ocbuf, ifbuf->length, 0);
+
+
+	if (nb_coefs & 1) {
+		for (k = 0; k < ifbuf->length; k++) {
+			fir->stater[fir->index] = ifbuf->data[k];
+			fir->index = (fir->index + 1) & mask;
+			acc = 0.0f;
+			j = start_index;
+			for (i = 0; i < nb_coefs; i++) {
+				acc += coefs[i] * fir->stater[j];
+				j = (j+1) & mask;
+			}
+			ocbuf->data[k].imag = acc;
+			ocbuf->data[k].real = fir->stater[middle_index];
+			start_index = (start_index + 1) & mask;
+			middle_index = (middle_index + 1) & mask;
+		}
+	}
+	else {
+		for (k = 0; k < ifbuf->length; k++) {
+			fir->stater[fir->index] = ifbuf->data[k];
+			fir->index = (fir->index + 1) & mask;
+			acc = 0.0f;
+			j = start_index;
+			for (i = 0; i < nb_coefs; i++) {
+				acc += coefs[i] * fir->stater[j];
+				j = (j+1) & mask;
+			}
+			ocbuf->data[k].imag = acc;
+			ocbuf->data[k].real = (fir->stater[middle_index] + fir->stater[(middle_index - 1) & mask]) / 2.0f;
+			start_index = (start_index + 1) & mask;
+			middle_index = (middle_index + 1) & mask;
+		}
+	}
+
+	return ocbuf;
+}
+
+
+#endif
+