;******************************************************************************
;* FILENAME                                                                   *
;*   IIR_filters_second_order_asm.asm                                         *
;*                                                                            *
;* DESCRIPTION                                                                *
;*   Assembly language implementation of second order IIR filter for the      *
;*   TMS320C5416 DSK.                                                         *
;*                                                                            *
;*  Revision: 1.00                                                            *
;*  Author: Richard Sikora                                                    *
;*                                                                            *
;*----------------------------------------------------------------------------*
;*  HISTORY                                                                   *
;*  Revision: 1.01                                                            *
;*   3rd December 2002. Shift increased from 3 to 7 to avoid overload on      *
;*   bandpass filters.                                                        *
;*  Revision: 1.00                                                            *
;*   21st November 2002. Created by Richard Sikora from                       *
;*   iir_filters_second_order.c                                               *
;*                                                                            * 
;******************************************************************************

	.mmregs
FP	.set	AR7
	
; Numerator coefficients 
B0 	.set 0
B1  .set 1
B2  .set 2

; Denominator coefficients 
A0  .set 3
A1  .set 4
A2  .set 5	

	.sect	".text"
	.bss	_delay,3,0,0

	.global	_second_order_IIR_asm_initialise

	.global	_second_order_IIR_direct_form_II_asm


;***************************************************************
;* FUNCTION DEF: _second_order_IIR_asm_initialise              *
;***************************************************************

_second_order_IIR_asm_initialise:

        ST        #0,*(_delay)          ; |17| 
        ST        #0,*(_delay+1)        ; |18| 
        ST        #0,*(_delay+2)        ; |19| 
        NOP
        NOP

        FRET      ; Far return


;***************************************************************
;* FUNCTION DEF: _second_order_IIR_direct_form_II_asm          *
;***************************************************************

; The value &coefficients[0] is passed to this function in accumulator A.
; Use Accumulator A to hold variable temp
; Order of coefficients is B0, B1/2, B2, A0, A1/2, A2

_second_order_IIR_direct_form_II_asm:

        PSHM      ST0
        PSHM      ST1
        PSHM      AR3
        PSHM      AR4
 
        SSBX      FRCT             ; Extra shift to right
        SSBX      SXM              ; Sign-extension mode on 
        SSBX      OVM              ; Prevent overflow
        
        ADD       #A0, A           ; Add offset A0 to &coefficients[0]                                   
        STLM      A, AR3           ; AR3 points to coefficients[A0]
        STM       #_delay+1, AR4   ; AR4 points to delay[1]  

        LD        *SP(2+4),T       ; Copy input to T register

;  A = (((long)coefficients[A0] * input ) >> 7 ); /* Divide by 128 */

        MPY       *AR3+,A          ; A = input * coefficients[A0] 
        SFTA      A,#-7,A          ; A >>= 7; 

        MAS       *AR3 , *AR4, A   ; A -= coefficients[A1] * delay[1]

        MAS       *AR3+, *AR4+, A  ; A -= coefficients[A1] * delay[1] 

        MAS       *AR3-, *AR4 , A  ; A -= coefficients[A2] * delay[2]

;  temp >>= 15;  /* temp /= coefficients[A0] */
;  delay[0] = (signed int) temp;
        
        STH       A, *(_delay)     ; delay[0] = A >> 16 

                                   ; AR3 points to coefficients[A1] 
        MAR *AR3-                  ; AR3 points to coefficients[A0]
        MAR *AR3-                  ; AR3 points to coefficients[B2] 


        MPY       *AR3-, *AR4-, A  ; A = coefficients[B2] * delay[2] 

        MAC       *AR3 , *AR4 , A  ; A += coefficients[B1] * delay[1]

        MAC       *AR3-, *AR4-, A  ; A += coefficients[B1] * delay[1]

        MAC       *AR3 , *AR4+, A  ; A += coefficients[B0] * delay[0]

        DELAY     *AR4-            ; delay[2] = delay[1]
        DELAY     *AR4             ; delay[1] = delay[0]

        SFTA      A, #(-16+7), A   ; A /= coefficients[A0] * 128

        POPM      AR4              ; Restore registers 
        POPM      AR3
        POPM      ST1              ; Restore FRCT, OVM, SXM  
        POPM      ST0
         
        FRET                       ; Far return 


;******************************************************************************
;* End of iir_filters_second_order_asm.asm                                    *
;******************************************************************************
