fir_filter.s

a good project in embedded system

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;
; FILE:	 FIR_Filter.s
;
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;


; ..............................................................................
; offsets into FIR Filter structure (entries are word wide -- hence +=2)

            .equ oNumTaps,   0    ; number of filter coefficients 

		.equ oTapsBase,  2    ; base address of filter coefficients
                                      ;     if coefficients are in data memory
                                      ;     must be an even address
                                      ; base address offset from program page boundary
                                      ;     if coefficients are in program memory
    
		.equ oTapsEnd,   4    ; end address of filter coefficients
                                      ;     must be an odd address
                                      ; end address offset from program page boundary
                                      ;     if coefficients are in program memory 
 
		.equ oTapsPage,  6    ; 0xFF00 if coefficients are in data space
                                      ; coefficient buffer page number
                                      ;     if coefficients are in program memory
 
		.equ oDelayBase, 8    ; base address of delay buffer
		.equ oDelayEnd, 10    ; end address + 1 of delay buffer
		.equ oDelayPtr, 12    ; starting value of delay pointer


; ..............................................................................
					
; -----------------------------------------------------------------------------
; -----------------------------------------------------------------------------
; Block FIR implementation for Microchip's dsPIC30F 16-bit MCU.
; This file should be assembled and linked against filter coefficients
; generated by dsPicFD -- filter design software by Momentum Data Systems. 
;
; 
; Module Re-entrancy:
;     Module re-entrancy is not supported
;
;
; Input:  for routine 	BlockFIRFilter
;	
;	  w0 = pointer to FIR filter structure
;	  w1 = pointer to input sample buffer
;	  w2 = pointer to output sample buffer
;	  w3 = number of output samples to generate
;
;
; System Resource usage:
;	  w0, w3               used not restored
;         w1, w2               used not restored
;	  w4, w5, w6           used not restored
;	  w8, w10              used, saved and restored
;         Acc A                used not restored
;         MODCON               register used for modulo addressing, saved and restored
;       XMODSTRT, XMODEND      used, saved and restored
;       YMODSTRT, YMODEND      used, saved and restored
;       CORCON                 used, saved and restored
;
; Input: for FIRFilterInit
;	  w0 = pointer to filter structure
;
;
; DO and REPEAT instruction usage
;     1 level DO instruction
;     1 level REPEAT instruction
;
;
; Module Program Memory Size
;     _BlockFIRFilter:	52
;     _FIRFilterInit:	6
;
;
; Module Cycle Count
;     _BlockFIRFilter:	48 + N*(4 + M)	(+2 if PSV)
;     _FIRFilterInit:	7 + M
; with N: number of samples per block, M: number of taps in filter.
;
; -----------------------------------------------------------------------------
; -----------------------------------------------------------------------------

		.text
		.global _BlockFIRFilter

_BlockFIRFilter:
; ..............................................................................
; Entry context save of selected registers

         PUSH  MODCON            ; save context of MODCON
         PUSH  w8                ; save context of w8
         PUSH  w10               ; save context of w10
         PUSH  XMODSRT           ; save context of XMODSRT
         PUSH  XMODEND           ; save context of XMODEND
         PUSH  YMODSRT           ; save context of YMODSRT
         PUSH  YMODEND           ; save context of YMODEND
         PUSH  CORCON            ; save context of CORCON
         PUSH  PSVPAG            ; save context of PSVPAG



; ..............................................................................; 
; Check if filter-taps are stored in Program Space or Data Space and enable
; PSV and set up PSVPAG accordingly

         MOV   [w0+oTapsPage], W10
         MOV   #0xFF00, W8
         CP    W8, W10           ; perform w8-w10 (if w10 = FF00 then do not enable PSV)
         BRA   Z, no_psv         ; branch if compare true (coefficients are in data space) 
         MOV   #0x00B4,W8        ; Enable enable Program Space Visibility, 
                                 ; Accumulator A Saturation and Data Space write Saturation
                                 ;    as bits 2, 5 and 7 are set in CORCON
                                 ; Also note bits 2, 5 and 7 are in low order byte which is
                                 ;   the first byte of a 2 byte word in a little endian 
                                 ;   processor such as the dsPIC30
                                 ; also, enable unbiased (convergent) rounding mode,
                                 ; 1.39 saturation enabled, fractional mult.
         MOV   w10, PSVPAG       ; PSVPAG = Program Space page containing filter taps
         BRA   SetupPointers     

no_psv:
         MOV   #0x00B0,w8        ; Enable Accumulator A Saturation and 
                                 ; Data Space write Saturation 
                                 ;    as bits 5 and 7 are set in CORCON
; ..............................................................................;
; Setup pointers and modulo addressing

SetupPointers:
         MOV   W8, CORCON               ; set PSV and saturation options

         MOV  [w0+oTapsEnd],w8	
         MOV  w8, XMODEND	        ; XMODEND = end address of filter coefficients		
         MOV  [w0+oTapsBase],w8    ; w8 = base address of taps/filter coefficients
         MOV  w8, XMODSRT		; XMODSRT = base address of filter coefficients

         MOV  [w0+oDelayEnd],w10	
         MOV  w10, YMODEND		; YMODEND = end address of delay line
         MOV  [w0+oDelayBase],w10	
         MOV  w10, YMODSRT		; YMODSRT = base address of delay line

         MOV  [w0+oNumTaps],w4

         SUB  w4,#3, w4                 ; w4 = numTaps-3 (for do loop control)
		 
		 MOV   #0xC0A8, w10             ; set XMD = W8 and YMD = W10
         MOV   w10, MODCON	        ; enable X & Y Modulus   	
         
         DEC  w3,w3                     ; w3 is do loop counter - set to 1 less than count
         MOV  [w0+oDelayPtr],w10	   ; w10 = pointer to current delay sample
         				
;..............................................................................		 
; Perform Block FIR filtering
		 
         
         DO   w3, blockLoop             ; loop until all input samples have been processed
         MOV  [w1++],[w10]              ; store new sample into delay line

; clear a, prefetch tap and sample pair, update ptrs
         CLR   a, [w8]+=2, w5, [w10]+=2, w6

         REPEAT w4                      ; perform macs (except for last two)
         MAC  w5*w6, a, [w8]+=2, w5, [w10]+=2, w6

         MAC  w5*w6, a, [w8]+=2, w5, [w10], w6	; perform second-to-last MAC
         MAC  w5*w6, a				; perform last MAC


		; round and store result in AccA to output buffer
blockLoop:
         SAC.R  a,[w2++]            ; this is the last instruction in the loop controlled
                                    ; by W3                        

         MOV  w10,[w0+oDelayPtr]    ; update delay line pointer
                                    ; note: that the delay line pointer can have multiple 
                                    ; wraps depending on the number of input samples
;---------------------------------------------------------------------------------

; ..............................................................................		 
; Cleanup(Context restore of selected registers)
         CLR MODCON                 ; disable modulo addressing
         NOP
         POP  PSVPAG                ; restore context of PSVPAG
         POP  CORCON                ; restore context of CORCON
         POP  YMODEND               ; restore context of YMODEND
         POP  YMODSRT               ; restore context of YMODSRT
         POP  XMODEND               ; restore context of XMODEND
         POP  XMODSRT               ; restore context of XMODSRT
         POP  w10                   ; restore context of w10
         POP  w8                    ; restore context of w8
         POP  MODCON                ; restore context of MODCON
         NOP
         RETURN                     ; exit from  _BlockFir
         
		
;-----------------------------------------------------------------------------
; Input:		
;		w0 = pointer to filter structure

		.text
		.global _FIRFilterInit

_FIRFilterInit:
		
         MOV [w0+oDelayBase], w1    ; w1 = base address of delay line
         MOV [w0+oNumTaps], w0      ; w0 = num taps
		
         DEC w0,w0			; w0 -=1		
         REPEAT w0
         CLR [w1++]                 ; init delay line (i.e. fill with zeros)

         RETURN                     ; exit from _InitFir


;-----------------------------------------------------------------------------		
				

.end