POPM      AR3
        POPM      ST1
        POPM      ST0                    

        FRET                             ; Far return                              


;******************************************************************************
;* FUNCTION DEF: _FIR_dual_filter_asm                                         *
;******************************************************************************
;*                                                                            *
;* FIR filter for producing both high pass and low pass filter from a single  *
;* set of coefficients.                                                       *
;*                                                                            *
;* Bass and treble returned in AH and AL                                      *
;*                                                                            *
;******************************************************************************

_FIR_dual_filter_asm:

	    PSHM      ST0                   ; Any flags set here have no side-effects
	    PSHM      ST1
        PSHM      AR3
        PSHM      AR4
        SSBX      OVM                   ; Prevent overflow 
        SSBX      FRCT                  ; Shift right to remove extra sign bit 
        FRAME     #-1
        STLM      A,AR3                 ; AR3 now points to _coefficients

; Start by shuffling values in buffer along and inserting new value at buffer3[0]

        STM       #_buffer3+N-2, AR4    ; AR4 points to buffer3[50]                 
	    RPT  #(N-2)                     ; Shuffle all the values. # is important!
	    DELAY *AR4-

        MVDK      *SP(4+3),*(_buffer3)   ; New input to beginning of buffer  
        STM       #_buffer3, AR4         ; AR4 now points to buffer3[0]

; Series of multiplications with accumulation. Last with rounding.  

        RPTZ  A, #(N-2)                 ; Clear A then repeat 49 times
        MAC *AR3+, *AR4+, A             ; Multiply and accumulate in A  
         
        MACR *AR3+, *AR4+, A            ; Round up the last one. This makes AL = 0.                                    

        SFTA A, -16, B                  ; Remove fractional part of product and copy 
                                        ; to accumulator B. AH also contains product.  
        NEG B                           ; product = -product  

        STM #_buffer3+(N-1)/2, AR4      ; AR4 points to mid point i.e. buffer3[25] 

        ADD *AR4, 0, B, B               ; Mid point + (- product)
                                        ; If AH contains bass, BL contains treble.
        
        AND #0FFFFh, B                  ; Ensure BH is zero.

        OR B, 0, A                      ; Return A OR B in Accumulator A

        FRAME     #1 
        POPM      AR4 
        POPM      AR3
        POPM      ST1
        POPM      ST0                    

        FRET                             ; Far return                                
    

;*******************************************************************************
;* FUNCTION DEF: _FIR_dual_filter_variable_asm                                 *
;*******************************************************************************
;*                                                                             *
;* When function is called from C code:                                        *
;* Accumulator contains address of coefficients                                *
;* *SP(8) contains latest input to filter                                      *
;* *SP(9) contains length of filter.                                           *
;*                                                                             *
;* RETURNS: Bass and Treble in AH and AL                                       *
;*******************************************************************************

_FIR_dual_filter_variable_asm:

        PSHM      AR1
        PSHM      ST0        ; Save FRCT, OVA and C 
        PSHM      ST1        
        PSHM      AR3        ; Save AR3 and AR4
        PSHM      AR4

        FRAME     #-1
        
        SSBX      OVM       ; Prevent overflow causing sign change 
        SSBX      FRCT      ; Extra bit of resolution on accumulation 

        ; Save address of coefficients in accumulator A to AR3

        STLM      A, AR3 

        ; Test for length less than 3	
        SSBX      SXM
        LD        #3,A
        SUB       *SP(5+4),A             
        BC        L1,ALEQ                
                 
        ; Length less than 3. Set to 3.
        ST        #3,*SP(5+4)             

        B         L2                     

L1:    
        ; Test for length greater than N	  
        LD        #N,A
        SUB       *SP(5+4),A               
        BC        L2,AGEQ               
                	   
	    ; Length greater than N. Set to N
        ST        #N,*SP(5+4)             
L2:    
        ; Shuffle values in buffer one place along

        STM #_buffer4+N-2, AR4        ; AR4 points to buffer4[N-2]   

        RPT #(N-2)
	    DELAY *AR4-
	
	    ; Save new input in buffer[0]
	
	    MVDK      *SP(4+4),*(_buffer4)       
	
	    ; Calculate ( N - length ) / 2
	
        LD        #N,A
        SUB       *SP(5+4),A
	    SFTA      A, #-1, A         ; Accumlator A contains ( N - length ) / 2
	
	    ADD       #_buffer4, 0, A
	    STLM      A, AR4            ; AR4 contains address of buffer4[(N-length)/2]
	
	    LDM       AR3, B            ; Copy starting address of coefficients to acc B
	
        LD        #N,A
        SUB       *SP(5+4),A              	    
	    SFTA      A, #-1, A      ; Accumulator A contains ( N - length ) /2 	     
	
	    ADD       B, 0, A              
	    
	    STLM      A, AR3         ; AR3 points to coefficients[( N - length ) / 2 ] 
	
	    LD   *SP(5+4), A         ; Copy length to accumulator A      
                                 ; Do not use LDM here! 
	    ADD #-2, A               

	    STL   A, *SP(5+4)        ; Length -= 2 for correct number of repeats

	    LD #0 , A                ; Clear accumulator

        ; Multiplications with accumulation controlled by length  

	    RPT *SP(5+4)               ; Repeat following instruction controlled by length 
        MAC *AR3+, *AR4+, A        ; Multiply and accumulate in A  
         
        MACR *AR3+, *AR4+, A       ; Round up the last one. AL = 0.                                    

        SFTA A, -16, B             ; Remove fractional part of product and copy 
                                   ; to accumulator B. AH also contains product.  
        NEG B                      ; product = -product  

        STM #_buffer4+(N-1)/2, AR4 ; AR4 points to mid point i.e. buffer4[(N-1)/2] 

        ADD *AR4, 0, B, B          ; Mid point + (- product)
                                   ; If AH contains bass, BL contains treble.
        
        AND #0FFFFh, B             ; Ensure BH is zero.

        OR B, 0, A                 ; Return A OR B in Accumulator A
	
        FRAME     #1               ; Restore stack frame 
        
        POPM      AR4              ; Restore registers
        POPM      AR3
        POPM      ST1              ; Restore status of FRCT, OVA and C
        POPM      ST0
        POPM      AR1                 
        FRET                       ; Far return   

;******************************************************************************
;* End of FIR_filters_asm.asm                                                 *
;******************************************************************************