/* ///////////////////////////////////////////////////////////////////////////////////
/    FIR_Demo.asm                          21161 EZ-KIT LITE SIMDFIR digital filter  /
/                                                                                    /
/    Receives input from the AD1836 A/D's via the 21161 serial port (SPORT1),        /
/    Filters the data using a Finite Impuse Response Filter, and then stores the     /
/    output for the SPORT1 ISR to transmit the data back to the AD1836 D/A's.        /
/                                                                                    /
/    The digital filter coefficients are in the files "high1.dat" and "low1.dat"     /
/                                                                                    /
/    The samples from the AD1836 are 24-bits and are are converted to                /
/    floating point numbers.  The values are also scaled to the range +/-1.0         /
/    with the integer to float conversion (f0 = float r0 by r1).                     /
/                                                                                    /
/    The digital filtering takes place in the SPORT1 transmit interrupt service      /
/    routine.                                                                        /
/                                                                                    /
/    See the "ADSP-21000 Family Applications Handbook Vol. 1" for more information   /
/    regarding the implementation of digital filters (chapter 4)                     /
/                                                                                    /
/////////////////////////////////////////////////////////////////////////////////// */

/* ADSP-21161 System Register bit definitions */
#include 	"def21161.h"


#define FILTER_TAPS 130
#define ONEHALF_FILTER_TAPS 65


.segment /dm    dm_data;
.VAR	dummy_variable=0;
.ALIGN 2;
.VAR    dline [FILTER_TAPS];       				/* k[n] zapamietanych probek */
.VAR	dummy_variable2 = 0;
.VAR	filter_counter = 2;						/* filtr dolno przepustowy */

.endseg;

/*---------------------------------------------------------------------------*/

.segment /pm	  pm_data;

.ALIGN 2;
.VAR    coef_lo_300Hz[FILTER_TAPS] = "coeffs129_300Hz.dat";  	/* gorno przepustowy filtr, deklaracja wspó³czynników*/
.ALIGN 2;
.VAR	coef_lo_600Hz[FILTER_TAPS] = "coeffs129_600Hz.dat";		/* dolno przepustowy filtr, deklaracja wspó³czynników*/		
.ALIGN 2;
.VAR    coef_hi_4kHz[FILTER_TAPS] = "coeffs129_4kHz.dat";   	/* gorno przepustowy filtr, deklaracja wspó³czynników*/
.ALIGN 2;
.VAR	coef_hi_8kHz[FILTER_TAPS] = "coeffs129_8kHz.dat";		/* dolno przepustowy filtr, deklaracja wspó³czynników*/		


				
.endseg;


.segment /pm pm_code;

.GLOBAL		init_fir_filter;
.GLOBAL		fir;
.GLOBAL		change_filter_coeffs;

.EXTERN		RX_left_flag;
.EXTERN		Left_Channel_In1;
.EXTERN		Right_Channel_In1;
.EXTERN		Left_Channel_Out0;
.EXTERN		Right_Channel_Out0;
.EXTERN		Left_Channel_Out1;
.EXTERN		Right_Channel_Out1;

init_fir_filter:
	bit set mode1 CBUFEN;
	nop;

	/* inicjalizacja DAGS */
	b0 = dline;     	l0 = @dline;    		/* wskaznik do probek zapamietanych (tablicy) */
 	m1 = 1; m2 = -1; m3 = 2;

	b9 = coef_hi_4kHz;	l9 = @coef_hi_4kHz;   	/* wsaznik do wspolczynnikow gorno przepostowego filtru   */
	m9 = 2;

	B3=dline;	l3=@dline;	r12=0; 				/* czyszczenie tablicy */
   	lcntr=FILTER_TAPS, do clear until lce;
clear:    dm(i3,2)=r12;

   	rts;
init_fir_filter.end:

/* //////////////////////////////////////////////////////////////////////////////// *
 *                           FIR  procedura                                   *
 * //////////////////////////////////////////////////////////////////////////////// */

fir:
	/* dla lewego kanalu */
	r2 = -31;        						/* skala +/-1.0 */                           
	r0 = DM(Left_Channel_In1);				/* pobranie lewej probki */
	r1 = DM(Right_Channel_In1);				/* pobranie prawej probki */
	f0 = float r0 by r2 ;					/* konwersja na zmiennoprzecinkowa */		
	f1 = float r1 by r2 ;					/* konwersja na zmiennoprzecinkowa */
	
	f8 = f0;								/* kopiowanie na wyjscie w trybie bypass */
	f9 = f1;								/* kopiowanie na wyjscie w trybie bypass */
	f0 = f0 + f1;							/* sumowanie obydwu kanalow */
	f2 = 0.7;
	f0 = f0 * f2; 							/* obnirzenie glosnosci */

	if flag2_in jump exit_filter;

do_filtering:
//operacje wykonywane na 32  bitiwym slowie za pomoca rejestru kolowego

	bit clr mode1 BDCST9;						

	// filtrowanie na lewym i prawy kanale
	s0 = dm(i0, m1);							/* przeniesienie wskaznika do delay[1] */

	
	bit set mode1 PEYEN;	
	
	s0 = dm(i0, m2);							/* zaladowanie s0 wartoscia  delay[1] dla SIMD , przeniesienie wskaznika do delay[0] */

	dm(i0,m3)=f0, f4 = pm(i9,m9);				/* przeksztalcenie probki do lini opuzniajacej, zakończone obliczenia zapisane na koniec buffer + 1 */
												/* do kompenasacji bufora kolowego dochodzi opis obydwu odczytanych wspolczynnikow */										

	f8=f0*f4, f0=dm(i0,m3), f4=pm(i9,m9);		/* probka * wspolczynnik, 2 probki, 2 wspolczynniki */
	f12=f0*f4, f0=dm(i0,m3), f4=pm(i9,m9);		/* probka * wspolczynnik, 2 probki, 2 wspolczynniki */

	lcntr= ONEHALF_FILTER_TAPS - 3, do macloop until lce;	/* FIR pentla */
macloop:   f12=f0*f4, f8=f8+f12, f0=dm(i0,m3), f4=pm(i9,m9);	/* probka * wspolczynnik, 2 probki, 2 wspolczynniki, accum */

	f12=f0*f4, f8=f8+f12, s0=dm(i0,m2);	   		/* probka * wspolczynnik, accum, przesuniecie wskaznika na starsza probke */
	f8=f8+f12;								

	bit clr mode1 PEYEN;
	f12 = s8;									/* przeniesienie calosci PEy  do PEx rejestru  */
	f8 = f8 + f12;								/* ostatni accum... i PEx i PEy rezultat */

exit_filter:
	r1 = 31;									/* wspó³czynnik skali */
	r8 = fix f8 by r1;							/* konwersja do fixed point */
	DM(Left_Channel_Out0) = r8; 				/* wyslanie rezyltatu do AD1836 DACs */
	DM(Right_Channel_Out0) = r8;
	DM(Left_Channel_Out1) = r8; 			
	DM(Right_Channel_Out1) = r8;

	rts;
fir.end:


change_filter_coeffs:
    bit set mode1 SRRFH;				
	NOP; 								/* jeden cykl opoznienia do wpisania do rejestru mode1    */

	r11 = 4;
	r10 = DM(filter_counter);			/* pobranie licznika z pamieci */
	r10 = r10 + 1;						/* inkramentacja */
	comp (r10, r11);					/* porownanie obecnego stanu licznika z maksymalnym */
	if ge r10 = r10 - r10;				/* jesli licznik rowny maksymalnej wartosci zeruj go */
	DM(filter_counter) = r10;			/* zapisanie nowego stanu licznika */

	r15 = pass r10;						
	if eq jump filter_select_2;			/* przyrownaj licznik == 0 */
	r10 = r10 - 1;
	r10 = pass r10;						
	if eq jump filter_select_3;			/* przyrownaj licznik == 1 */
	r10 = r10 - 1;
	r10 = pass r10;						
	if eq jump filter_select_4;			/* przyrownaj licznik == 2 */

filter_select_1:						/* domyslny filtr */
	b9 = coef_lo_300Hz;	l9 = @coef_lo_300Hz;   	/* ladownie wspolczynnikow 300 Hz dolnoprzepustowego filtru   */
	ustat1=dm(IOFLAG);
	bit clr ustat1 0x3E;						/* zapalenie diody4 */
	bit set ustat1 0x01;
	dm(IOFLAG)=ustat1;
    jump exit_IRQ_routine;

filter_select_2:
	b9 = coef_lo_600Hz;	l9 = @coef_lo_600Hz;	/* ladownie wspolczynnikow 300 Hz dolnoprzepustowego filtru   */
	ustat1=dm(IOFLAG);
	bit clr ustat1 0x3D;						/* zapalenie diody 5 */
	bit set ustat1 0x02;
	dm(IOFLAG)=ustat1;		
    jump exit_IRQ_routine;

filter_select_3:					/* ladownie wspolczynnikow 4 kHz gornoprzepustowego filtru   */
	b9 = coef_hi_4kHz;	l9 = @coef_hi_4kHz;   	
	ustat1=dm(IOFLAG);
	bit clr ustat1 0x3B;						/* zapalenie diody6 */
	bit set ustat1 0x04;
	dm(IOFLAG)=ustat1;
    jump exit_IRQ_routine;

filter_select_4:
	b9 = coef_hi_8kHz;	l9 = @coef_hi_8kHz;   		/* ladownie wspolczynnikow 8000 Hz gornoprzepustowego filtru   */
	ustat1=dm(IOFLAG);
	bit clr ustat1 0x37;						/* zapalenie diody8 */
	bit set ustat1 0x08;
	dm(IOFLAG)=ustat1;		

exit_IRQ_routine:	
	rti(db);
	bit clr mode1 SRRFH;						
	nop;
change_filter_coeffs.end:

.endseg;