;************************************************************************
;*
;* Copyright:
;*	Freescale Semiconductor, INC. All Rights Reserved.  
;*  You are hereby granted a copyright license to use, modify, and
;*  distribute the SOFTWARE so long as this entire notice is
;*  retained without alteration in any modified and/or redistributed
;*  versions, and that such modified versions are clearly identified
;*  as such. No licenses are granted by implication, estoppel or
;*  otherwise under any patents or trademarks of Freescale Semiconductor, 
;*  Inc. This software is provided on an "AS IS" basis and without warranty.
;*
;*  To the maximum extent permitted by applicable law, FREESCALE 
;*  DISCLAIMS ALL WARRANTIES WHETHER EXPRESS OR IMPLIED, INCLUDING 
;*  IMPLIED WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR
;*  PURPOSE AND ANY WARRANTY AGAINST INFRINGEMENT WITH REGARD TO THE 
;*  SOFTWARE (INCLUDING ANY MODIFIED VERSIONS THEREOF) AND ANY 
;*  ACCOMPANYING WRITTEN MATERIALS.
;* 
;*  To the maximum extent permitted by applicable law, IN NO EVENT
;*  SHALL FREESCALE BE LIABLE FOR ANY DAMAGES WHATSOEVER (INCLUDING 
;*  WITHOUT LIMITATION, DAMAGES FOR LOSS OF BUSINESS PROFITS, BUSINESS 
;*  INTERRUPTION, LOSS OF BUSINESS INFORMATION, OR OTHER PECUNIARY
;*  LOSS) ARISING OF THE USE OR INABILITY TO USE THE SOFTWARE.   
;* 
;*  Freescale assumes no responsibility for the maintenance and support
;*  of this software
;*************************************************************************
;*
;*  FILE NAME: fir16.s
;*
;*  PURPOSE:  FIR16 routines definition
;*
;*  AUTHOR: Andrey Butok
;*		FIR16 function optimized for eMAC by Igor Drozdinsky
;*
;***********************************************************************
.text
;#define __EMAC_H
;#include "emac.h"
;#ifdef __FRACT_M
#define __FMAC_SR16 0x00000070
;#else
;#define __FMAC_SR16 0x00000000
;#endif

	.XDEF	_FIR16_EMAC
	.extern _malloc
	.extern _free


;******************************************************
;* NAME: void FIR16( struct tFirStruct *pFIR, Frac16* pX, Frac16* pY, unsigned int n)
;*
;* DESCRIPTION: Computes a Infinite Impulse Response (IIR) filter for a array of 16-bit fractional data values.
;******************************************************
;* Used registers:
;*	a2       pFIR	- Pointer to a data structure containing private data for the IIR filter
;*  d1		 Old MAC status register
;*	d2, d3	 iCurX0, iCurX1, iCurX2, iCurX3	- Curent X
;*	d4		 iCurA0, iCurA1	- Curent coefficients
;*	d5		 j	- inner loop counter
;*	d7		 i	- outer loop counter
;*	d6		 tmp	- saved value for inner loop counter calculation
;*	a0       pX		- Pointer to the current X for outer loop
;*	a1       pY		- Pointer to the current Y for outer loop
;*	a4       pCurX	- Pointer to the current X for inner loop
;*	a4       pCurHistory	- Pointer to the current element of history buffer
;*	a3       pCurCoef	- Pointer to the current coefficient
;*	a6       pFirCoef - Pointer to the coefficients -> {a0,a1,a2...}
;*	ACC0     iOut0	- Accumulator 0
;*	ACC1     iOut1	- Accumulator 1
;*	ACC2     iOut2	- Accumulator 2
;*	ACC3     iOut3	- Accumulator 3
;******************************************************
_FIR16_EMAC:

;---=Saving values of used registers=---
	lea -60(a7), a7
	movem.l d0-d7/a0-a6, (a7)
	
;--== Saving old MAC status register to d1 ==--
	move.l	MACSR, d1
;--== Loading new MAC status register ==--
	move.l	#__FMAC_SR16, d0
	move.l	d0, MACSR
	
;---=Most useful parameters are moved from stack to registers.=---	
	move.l	68(a7), a0			;pX
	move.l	72(a7), a1			;pY
	move.l	64(a7), a2			;pIIR
	move.l	(a2), a6			;pIirCoef = pIIR->pIirCoef
	move.l	#0, ACC0
	move.l	#0, ACC1
	move.l	#0, ACC2
	move.l	#0, ACC3		
;---====== Begin of cycle of getting Y[1]..Y[N] (N = (pIIR->iIirCoefCount - 1) / 2)======---

	move.l	4(a2), d7			;
	subq.l	#1, d7				;tmp = i = pIIR->iIirCoefCount - 1;
	move.l	d7, d6				;
	
	
	asr.l	#2, d7
			
	beq		.EndD4Z				;if(i >> 2)
								;{
 		
;--==First N output samples computation==--
		
	move.l	d6, d7
	subq.l	#4, d7				;i -= 4;

;--==Computation without using history buffer==--
		
.BegD4:								;do{
			
;--== Next input samples loading	==--
	move.l	(a0), d2			;iCurX0 = pX[0]; icurX1 = pX[1];
	move.l	4(a0), d3			;iCurX2 = pX[2]; icurX3 = pX[3];
	
	movea.l	a0, a4				;pCurX = pX;
	lea		8(a0), a0			;pX += 4;
	
	move.l	a6, a3				;pCurCoef = pIirCoef;
;--== Next coefficient loading	==--
	move.w	(a3)+, d4			;iCurA0 = *pCurCoef;

;--== Input samples and first coefficient multiplications ==--
	mac.w	d4.l, d2.u, <<, ACC0	;iOut0 = iCurA0 * iCurX0;
	mac.w	d4.l, d2.l, <<, ACC1	;iOut1 = iCurA0 * iCurX1;
	mac.w	d4.l, d3.u, <<, ACC2	;iOut2 = iCurA0 * iCurX2;
	mac.w	d4.l, d3.l, <<, ACC3	;iOut3 = iCurA0 * iCurX3;

	
;--== The count of inner loops calculation ==-- 
	move.l	d6, d5				;
	sub.l	d7, d5				;j = tmp - i - 4;
	subq.l	#4, d5				;
								
	ble		.EndIn1				;while(j > 0)
								;{
	
.ForIn1Beg:
;--== Next coefficients loading	==--
	move.l	(a3)+, d4			;iCurA0 = *pCurCoef++;iCurA1 = *pCurCoef++;

.ForIn1:
	
;--== Three first input samples and coefficients multiplication ==--
	mac.w	d4.u, d2.u, <<, ACC1	;iOut1 += iCurA0 * iCurX0;
	mac.w	d4.u, d2.l, <<, ACC2 	;iOut2 += iCurA0 * iCurX1;
	mac.w	d4.u, d3.u, <<, ACC3	;iOut3 += iCurA0 * iCurX2;
	
;--== Loading two next input samples ==--	
	move.l	d2, d3					;iCurX3 = iCurX1; 	iCurX2 = iCurX0;
	move.l	-(a4), d2				;iCurX1 = *--pCurX; iCurX0 = *--pCurX;
	
;--== Fourth input sample and coefficient multiplication ==--
	mac.w	d4.u, d2.l, <<, ACC0	;iOut0 += iCurA0 * iCurX1;
		
;--== Next four input sample and coefficients multiplication ==--
	mac.w	d4.l, d2.u, <<, ACC0	;iOut0 += iCurA1 * iCurX0;
	mac.w	d4.l, d2.l, <<, ACC1	;iOut1 += iCurA1 * iCurX1;
	mac.w	d4.l, d3.u, <<, ACC2	;iOut2 += iCurA1 * iCurX2;
;--== Last mac instruction with next coefficients loading ==--
	mac.w	d4.l, d3.l, <<, (a3)+, d4, ACC3	;iOut3 += iCurA1 * iCurX3;
											;iCurA0 = *pCurCoef++;iCurA1 = *pCurCoef++;
		
	subq.l	#2, d5 					;	j -= 2;
	bgt		.ForIn1					;} //while(j > 0)
	
.EndIn1C:	
;--== Correcting pointer to coefficients after inner loop ==--
	lea		-4(a3), a3				;pCurCoef -= 4
	
.EndIn1:	
	
	move.l	12(a2), d5				;if(pIIR->iIirHistoryCount)
;---=Computation using history buffer==--
	beq		.BegNoHistory			;{

;--== The count of inner loops calculation ==-- 
	move.l	d7, d5					;
	addq.l	#4, d5					;j = i + 4;

	ble		.EndInH					;if(j > 0)
									;{

;--== Setting curent history buffer pointer ==--
	move.l  8(a2), a4				;
	adda.l	d6, a4					;
	adda.l	d6, a4					;pCurHistory = pIIR->pIirHistory + tmp * 2;
	

	btst	#0, d5					;if(j & 1)
	beq		.ForInHBeg				;{
	
;--== This part is executed only if j is odd ==--	
;--== Next coefficients loading	==--
	move.w	(a3)+, d4				;iCurA = *pCurCoef++;
	
;--== Next input sample loading	from history buffer==--
	move.w	d2, d3					;iCurX3 = iCurX2;
	move.w	-(a4), d2				;iCurX2 = iCurX1
	swap	d2						;iCurX1 = iCurX0
	swap	d3						;iCurX0 = *--pCurHistory;

	
	mac.w	d4.l, d2.u, <<, ACC0	;iOut0 += iCurA0 * iCurX0;
	mac.w	d4.l, d2.l, <<, ACC1	;iOut1 += iCurA0 * iCurX1;
	mac.w	d4.l, d3.u, <<, ACC2	;iOut2 += iCurA0 * iCurX2;
	mac.w	d4.l, d3.l, <<, ACC3	;iOut3 += iCurA0 * iCurX3;
	
	subq.l	#1, d5					;j -= 1;
	ble		.EndInH					;}
									;while(j > 0)
									;{
	
.ForInHBeg:								
;--== Next coefficients loading	==--
	move.l	(a3)+, d4				;iCurA0 = *pCurCoef++; iCurA1 = *pCurCoef++;

.ForInH:
	
	
;--== Three first input samples and coefficients multiplication ==--
	mac.w	d4.u, d2.u, <<, ACC1	;iOut1 += iCurA0 * iCurX0
	mac.w	d4.u, d2.l, <<, ACC2	;iOut2 += iCurA0 * iCurX1
	mac.w	d4.u, d3.u, <<, ACC3	;iOut3 += iCurA0 * iCurX2
	
	
;--== Loading two next input samples ==--	
	move.l	d2, d3					;iCurX3 = iCurX1; 	iCurX2 = iCurX0;
	move.l	-(a4), d2				;iCurX1 = *--pCurHistory; iCurX0 = *--pCurHistory;
			
	
;--== Fourth input sample and coefficient multiplication ==--
	mac.w	d4.u, d2.l, <<, ACC0	;iOut0 += iCurA0 * iCurX0
	
;--== Next Four input sample and coefficients multiplication ==--
	mac.w	d4.l, d2.u, <<, ACC0	;iOut0 += iCurA1 * iCurX0
	mac.w	d4.l, d2.l, <<, ACC1	;iOut1 += iCurA1 * iCurX1
	mac.w	d4.l, d3.u, <<, ACC2	;iOut2 += iCurA1 * iCurX2
;--== Last mac instruction with next coefficients loading ==--
	mac.w	d4.l, d3.l, <<, (a3)+, d4, ACC3	;iOut3 += iCurA1 * iCurX3
											;iCurA0 = *pCurCoef++;iCurA1 = *pCurCoef++;
	
	subq.l	#2, d5					;j -= 2;
	bgt		.ForInH					;} // while(j > 0)
	
.EndInH:	
	bra		.EndH					;} // if(pIIR->iIirHistoryCount)
									;else{

.BegNoHistory:

;---=Final computations if history buffer is void==--
;--== Next coefficients loading	==--
	move.l	(a3)+, d4				;iCurA0 = *pCurCoef++; iCurA1 = *pCurCoef++;
	
	mac.w	d4.u, d2.u, <<, ACC1	;iOut1 += iCurA0 * iCurX1
	mac.w	d4.u, d2.l, <<, ACC2	;iOut2 += iCurA0 * iCurX2
	mac.w	d4.u, d3.u, <<, ACC3	;iOut3 += iCurA0 * iCurX3
		
	mac.w	d4.l, d2.u, <<, ACC2	;iOut2 += iCurA1 * iCurX2
	mac.w	d4.l, d2.l, <<, ACC3	;iOut3 += iCurA1 * iCurX3
		
;--== Next coefficient loading	==--
	move.w	(a3)+, d4				;iCurA0 = *pCurCoef++;
	
	mac.w	d4.l, d2.u, <<, ACC3	;iOut3 += iCurA0 * iCurX3
	
									;} // else if (pIIR->iIirHistoryCount)
	
.EndH:

;--==Storing computed samples into the memory==--
				
	move.l	a6, a3					;pCurCoef = pIirCoef
	
	movclr.l	ACC0, d0			;*pY++ = iOut0
;#ifndef __FRACT_M	
;	swap	d0						;
;#endif	
	move.w	d0, (a1)+				;
			
	movclr.l	ACC1, d0			;*pY++ = iOut1
;#ifndef __FRACT_M	
;	swap	d0						;
;#endif	
	move.w	d0, (a1)+				;
	
	movclr.l	ACC2, d2			;*pY++ = iOut2
;#ifndef __FRACT_M	
;	swap	d2						;
;#endif	
	move.w	d2, (a1)+				;
	
	movclr.l	ACC3, d0			;*pY++ = iOut3
;#ifndef __FRACT_M	
;	swap	d0						;
;#endif	
	move.w	d0, (a1)+				;

	subq.l	#4, d7					;i -= 4;
	bgt		.BegD4					;}while(i > 0);
	
	bra		.EndD4

.EndD4Z:
	
	move.l	d6, d7					;
	beq		.EndTailH				;
	bra		.BegTailH				;
	
.EndD4:

	addq.l	#4, d7					; i += 4;
	
	beq		.EndTailH				;while(i)
									;{
	
;---=Computation of N % 4 last output samples==--
.BegTailH: