/*
* file firlib\src\fir_sp_sse_r4.c
* brief FIR filtering function in radix 4 vectorized C (single precision IEEE754)
* warning radix 4 input and coefficients arrays sizes, SSE4.1 extension support
* author
* date
*/

#include "firlib.h"

/*
* FIR filtering function in radix 4 vectorized C (single precision IEEE754)
* warning radix 4 input and coefficients arrays sizes, SSE4.1 extension support
*/
void fir_sp_sse_r4 (	const float * restrict xk, 	\
			const float * restrict a,	\
			float * restrict yk, 		\
			int na,				\
			int nyk){
	int i, j, idx;
	__m128 xmm128_xk0123, xmm128_xk1234, xmm128_xk2345, xmm128_xk3456;
	__m128 xmm128_a0123;
	__m128 xmm128_acc;
	xmm_t xmm128_dp0, xmm128_dp1, xmm128_dp2, xmm128_dp3;
	
	/* DEBUG section code for tests */
	//xmm_t debug;
	//debug.m128_vec = xmm128_<vector_under_test>;
	//printf("\nxmm xk0123 : %.6f %.6f %.6f %.6f", debug.m128_f32[0], debug.m128_f32[1], debug.m128_f32[2], debug.m128_f32[3]);
	
	/* input array loop */
	for (i=0; i<nyk; i+=4) {

		xmm128_acc = _mm_setzero_ps();

		/* FIR filter algorithm - dot product */
		for (j=0; j<na; j+=4){

			xmm128_a0123 = _mm_load_ps(&a[j]);				

			idx = j+i;
			xmm128_xk0123 = _mm_load_ps(&xk[idx]);	
			xmm128_xk1234 = _mm_loadu_ps(&xk[idx+1]);
			xmm128_xk2345 = _mm_loadu_ps(&xk[idx+2]);
			xmm128_xk3456 = _mm_loadu_ps(&xk[idx+3]);

			xmm128_dp0.m128_vec = _mm_dp_ps(xmm128_a0123, xmm128_xk0123, 0xff);
			xmm128_dp1.m128_vec = _mm_dp_ps(xmm128_a0123, xmm128_xk1234, 0xff);
			xmm128_dp2.m128_vec = _mm_dp_ps(xmm128_a0123, xmm128_xk2345, 0xff);
			xmm128_dp3.m128_vec = _mm_dp_ps(xmm128_a0123, xmm128_xk3456, 0xff);

		
			xmm128_dp0.m128_f32[1] = xmm128_dp1.m128_f32[0];
			xmm128_dp0.m128_f32[2] = xmm128_dp2.m128_f32[0];
			xmm128_dp0.m128_f32[3] = xmm128_dp3.m128_f32[0];
			xmm128_acc = _mm_add_ps(xmm128_acc, xmm128_dp0.m128_vec);
			
		}
		
		_mm_store_ps(&yk[i],xmm128_acc);
	}
}