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

#include "firlib.h"

/*
* FIR filtering function in radix 4 vectorized C (single precision IEEE754)
* warning radix 4 input and coefficients arrays sizes
*/
void fir_sp_opt_r4 (const float * restrict xk, 	\
					const float * restrict a,	\
					float * restrict yk, 		\
					int na,						\
					int nyk){
	int i, j;
	__float2_t acc1, acc2, acc3, acc4, acc5, acc6, acc7, acc8;
	__float2_t a10, a32;
	__float2_t xk10, xk21, xk32, xk43, xk54, xk65;

    _nassert(na >= 4);
    _nassert(na % 4 == 0);
    _nassert(nyk >= 4);
    _nassert(nyk % 4 == 0);
    _nassert((int) xk % 8 == 0);
    _nassert((int) yk % 8 == 0);
    _nassert((int) a % 8 == 0);

	for (i=0; i<nyk; i+=4) {
		acc1 = 0;
		acc2 = 0;
		acc3 = 0;
		acc4 = 0;
		acc5 = 0;
		acc6 = 0;
		acc7 = 0;
		acc8 = 0;
		
		for (j=0; j<na; j+=4){
            a10 = _amemd8_const(&a[j]);
            a32 = _amemd8_const(&a[j+2]);

            xk10 = _amemd8_const(&xk[j+i]);
            xk32 = _amemd8_const(&xk[j+i+2]);
            xk54 = _amemd8_const(&xk[j+i+4]);
            xk65 = _memd8_const(&xk[j+i+5]);
            xk21 = _ftod(_lof(xk32), _hif(xk10));
            xk43 = _ftod(_lof(xk54), _hif(xk32));

            acc1 = _daddsp(acc1, _dmpysp(xk10, a10));
            acc2 = _daddsp(acc2, _dmpysp(xk21, a10));
            acc3 = _daddsp(acc3, _dmpysp(xk32, a10));
            acc4 = _daddsp(acc4, _dmpysp(xk43, a10));
            acc5 = _daddsp(acc5, _dmpysp(xk32, a32));
            acc6 = _daddsp(acc6, _dmpysp(xk43, a32));
            acc7 = _daddsp(acc7, _dmpysp(xk54, a32));
            acc8 = _daddsp(acc8, _dmpysp(xk65, a32));
		}
        acc1 = _daddsp(acc1, acc5);
        acc2 = _daddsp(acc2, acc6);
        acc3 = _daddsp(acc3, acc7);
        acc4 = _daddsp(acc4, acc8);

        _amemd8(&yk[i])   = _ftod(_hif(acc2)+_lof(acc2), _hif(acc1)+_lof(acc1));
        _amemd8(&yk[i+2]) = _ftod(_hif(acc4)+_lof(acc4), _hif(acc3)+_lof(acc3));
	}
}