adaptive_filter_asm.asm

这是在TMS320C5416上实现的吉他发声程序

;******************************************************************************
;* FILENAME                                                                   *
;*   adaptive_filters_asm.asm                                                 *
;*                                                                            *
;* DESCRIPTION                                                                *
;*   Adaptive filter for the TMS320C5416 DSK.                                 *
;*   Implemented in assembly language because C code version is too slow      *
;*                                                                            *
;*  Revision: 1.00                                                            *
;*  Author: Richard Sikora                                                    *
;*                                                                            *
;*----------------------------------------------------------------------------*
;*  HISTORY                                                                   *
;*  Revision: 1.00                                                            *
;*  13th November 2002. Created by Richard Sikora from adaptive_filter.c      *
;*                                                                            *
;*----------------------------------------------------------------------------*
;*                                                                            *
;*  The number of filter elements X[N] is controlled by N                     *
;*  The rate of convergence of the filter is determined by BETA               * 
;*                                                                            * 
;******************************************************************************

	 .mmregs
FP	 .set	AR7

; Number of elements in filter X[]

N    .set   34

; Feedback amount. Affects convergence rate of filter.

BETA .set   560

; Storage on stack

INPUT   .set 9

; Variables used in function

	.bss	_X_1,N,0,0
	.bss	_X_2,N,0,0
	.bss	_W_1,N,0,0
	.bss	_W_2,N,0,0	
	.bss	_error_1,1,0,0
	.bss	_error_2,1,0,0	
    .bss    _desired,1,0,0

; Functions callable from C code

	.sect	".text"
	.global	_adaptive_filters_asm_initialise
	.global	_adaptive_filter_asm_1
    .global _adaptive_filter_asm_2

;******************************************************************************
;* FUNCTION DEFINITION: _adaptive_filter_initialise                           *
;******************************************************************************

_adaptive_filters_asm_initialise:

		PSHM       AR3            ; Keep original value of AR3
		
		LD         #0, A          ; Clear accumulator A
		
		STM        #_X_1, AR3   
		RPTZ       A, #(N-2)      ; Fill 0 to N-1. Less one for repeats
		STL        A, *AR3+       ; Fill X array with zeroes

		STM        #_X_2, AR3   
		RPTZ       A, #(N-2)      ; Fill 0 to N-1. Less one for repeats
		STL        A, *AR3+       ; Fill X array with zeroes
		
		STM        #_W_1, AR3
		RPT        #(N-2)
		STL        A, *AR3+       ; Fill Weights with zeroes

		STM        #_W_2, AR3
		RPT        #(N-2)
		STL        A, *AR3+       ; Fill Weights with zeroes

        STL        A, *(_error_1) ; Clear errors to zero
        STL        A, *(_error_2) 
		
		POPM       AR3            ; Restore original value of AR3 
		
		FRET			          ; Far return


;*******************************************************************************
;* FUNCTION DEFINITION: _adaptive_filter_asm_1                                 *
;*******************************************************************************

; 1st parameter in accumulator = desired
; 2nd parameter on stack = input

; Desired is copied from accumulator A to stack frame.

; Returns Y in AH, error in AL

_adaptive_filter_asm_1:

        PSHM       ST0                ; Keep original values of flags
		PSHM       ST1
		PSHM       AR3                ; Keep original values of auxiliary registers
		PSHM       AR4

		FRAME      #-3                ; Setup stack frame 
		
    	SSBX       SXM                ; Turn on sign-extension mode 
    	SSBX       FRCT               ; Extra shift for multiplications
    	SSBX       OVM                ; Prevent overflow
    	
    	LD         #0, ASM            ; Required for correct behaviour of ST || MPY
    	
        STL        A, *(_desired)     ; Copy desired from Accumulatr A to local storage 

        STM        #(_X_1 + N-2), AR3 ; AR3 points to X[N-2] 
        
        RPT        #(N-2)
        DELAY      *AR3-              ; Shuffle values along one place 
    
    	MVDK       *SP(INPUT), *(_X_1); Copy in latest value to X[0] 

        LD         *(_error_1), T     ; T = error 
        MPY        #BETA, A           ; A = error * BETA
        SFTA       A, -16, A          ; Move AH to AL

        STLM       A, T               ; T = error * BETA 

; Setup pointers to X[0] and W[0]  

        STM        #(_X_1), AR3       ; AR3 points to X[0]
        
        STM        #(_W_1), AR4       ; AR4 points to W[0]
         
        SUB        B, B               ; Clear accumulator B
        
        MPY        *AR3, A            ; A = error * BETA * X[]

; Perform Least Mean Squares calculations.

        STM        #(N-1), BRC        ; Set number of block repeats 

        RPTB       end_lms_1 - 1      ; Repeat instructions up to end_lms
                 
        LMS        *AR4, *AR3+        ; A = W[] + error * BETA * X[]  
                                      ; B += W[] * X[] 
        
        ST         A, *AR4+           ; Update W[]. Point to next W[]  
        ||         MPY *AR3, A        ; Last multiply is ignored

end_lms_1

        ; Build up return value of output of filter Y in AH and error in AL 
                                
        LD         B, -16, A          ; BH = Y. Copy Y to AL 

        NEG        A                  ; A = (-Y) 

        ADD        *(_desired), A     ; AL = Desired + (-Y) = error

        STL        A, *(_error_1)     ; Save error for next time through 

        PSHM       BH                 ; Copy Y in BH to AH
        POPM       AH                 ; Combined return value. AH = Y. AL = error
        
        FRAME      #3                 ; Restore stack frame
                   
        POPM       AR4                ; Restore auxiliary registers
        POPM       AR3
        POPM       ST1                ; Restore FRCT, SXM, OVM, C
        POPM       ST0        
                             
        FRET                          ; Far return       


;*******************************************************************************
;* FUNCTION DEFINITION: _adaptive_filter_asm_2                                 *
;*******************************************************************************

; 1st parameter in accumulator = desired
; 2nd parameter on stack = input

; Desired is copied from accumulator A to stack frame.

; Returns Y in AH, error in AL

_adaptive_filter_asm_2:

        PSHM       ST0                ; Keep original values of flags
		PSHM       ST1
		PSHM       AR3                ; Keep original values of auxiliary registers
		PSHM       AR4

		FRAME      #-3                ; Setup stack frame 
		
    	SSBX       SXM                ; Turn on sign-extension mode 
    	SSBX       FRCT               ; Extra shift for multiplications
    	SSBX       OVM                ; Prevent overflow
    	
    	LD         #0, ASM            ; Required for correct behaviour of ST || MPY
    	
        STL        A, *(_desired)     ; Copy desired from Accumulatr A to local storage 

        STM        #(_X_2 + N-2), AR3 ; AR3 points to X[N-2] 
        
        RPT        #(N-2)
        DELAY      *AR3-              ; Shuffle values along one place 
    
    	MVDK       *SP(INPUT), *(_X_2); Copy in latest value to X[0] 

        LD         *(_error_2), T     ; T = error 
        MPY        #BETA, A           ; A = error * BETA
        SFTA       A, -16, A          ; Move AH to AL

        STLM       A, T               ; T = error * BETA 

; Setup pointers to X[0] and W[0]  

        STM        #(_X_2), AR3       ; AR3 points to X[0]
        
        STM        #(_W_2), AR4       ; AR4 points to W[0]
         
        SUB        B, B               ; Clear accumulator B
        
        MPY        *AR3, A            ; A = error * BETA * X[]

; Perform Least Mean Squares calculations.

        STM        #(N-1), BRC        ; Set number of block repeats 

        RPTB       end_lms_2 - 1      ; Repeat instructions up to end_lms
                 
        LMS        *AR4, *AR3+        ; A = W[] + error * BETA * X[]  
                                      ; B += W[] * X[] 
        
        ST         A, *AR4+           ; Update W[]. Point to next W[]  
        ||         MPY *AR3, A        ; Last multiply is ignored

end_lms_2

        ; Build up return value of output of filter Y in AH and error in AL 
                                
        LD         B, -16, A          ; BH = Y. Copy Y to AL 

        NEG        A                  ; A = (-Y) 

        ADD        *(_desired), A     ; AL = Desired + (-Y) = error

        STL        A, *(_error_2)     ; Save error for next time through 

        PSHM       BH                 ; Copy Y in BH to AH
        POPM       AH                 ; Combined return value. AH = Y. AL = error
        
        FRAME      #3                 ; Restore stack frame
                   
        POPM       AR4                ; Restore auxiliary registers
        POPM       AR3
        POPM       ST1                ; Restore FRCT, SXM, OVM, C
        POPM       ST0        
                             
        FRET                          ; Far return       

;*******************************************************************************
;* End of adaptive_filter_asm.asm                                              *
;*******************************************************************************