iirtrans.s

dsPIC30系列单片机的DSP算法库

;*********************************************************************
;                                                                    *
;                       Software License Agreement                   *
;                                                                    *
;   The software supplied herewith by Microchip Technology           *
;   Incorporated (the "Company") for its dsPIC controller            *
;   is intended and supplied to you, the Company's customer,         *
;   for use solely and exclusively on Microchip dsPIC                *
;   products. The software is owned by the Company and/or its        *
;   supplier, and is protected under applicable copyright laws. All  *
;   rights are reserved. Any use in violation of the foregoing       *
;   restrictions may subject the user to criminal sanctions under    *
;   applicable laws, as well as to civil liability for the breach of *
;   the terms and conditions of this license.                        *
;                                                                    *
;   THIS SOFTWARE IS PROVIDED IN AN "AS IS" CONDITION.  NO           *
;   WARRANTIES, WHETHER EXPRESS, IMPLIED OR STATUTORY, INCLUDING,    *
;   BUT NOT LIMITED TO, IMPLIED WARRANTIES OF MERCHANTABILITY AND    *
;   FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE. THE     *
;   COMPANY SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL,  *
;   INCIDENTAL OR CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.  *
;                                                                    *
;   (c) Copyright 2003 Microchip Technology, All rights reserved.    *
;*********************************************************************

	; Local inclusions.
	.nolist
	.include	"dspcommon.inc"		; PSVPAG, COEFFS_IN_DATA,
						; kSof
	.list

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

; IIR Transposed filter structure access.
	.equ oNumSectionsLess1,0	; IIRTransposed->numSectionsLess1:(int)
					; number second order sections minus one
	.equ oCoeffs,(oNumSectionsLess1+kSof)
					; IIRTransposed->coeffsBase :
					; (fractional*)
					; pointer to filter coefficients
					; number of coefficients is 
					; 5 * number of second order sections
					; organized as {b0, b1, a1, b2, a2}
	.equ oPSVpage,(oCoeffs+kSof)	; IIRTransposed->CoeffsPage : (int)
					; page number in program memory if
					; coefficients are in program memory
					; 0xFF00 if not
	.equ oStates1,(oPSVpage+kSof)	; IIRTransposed->delayBase1 :
					; (fractional*)
					; prt to state variable (delay) 1
					; one word for every section
	.equ oStates2,(oStates1+kSof)	; IIRTransposed->delayBase2 :
					; (fractional*)
					; prt to state variable (delay) 2
					; one word for every section
	.equ oFinalShift,(oStates2+kSof)
					; IIRTransposed->finalShift : (int)
					; final shift count (left)
					; restores filter gain to 0 dB
					; shift count may be zero
					; if not zero, it is the number of bits
					; to shift the output to the left
					; negative value means shift left

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

	.section .libdsp, "x"

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;
; _IIRTransposed: Cascade of second order IIR Transposed Form II filtering.
; This file should be assembled and linked against filter coefficients
; generated by dsPicFD -- filter design software by Momentum Data Systems. 
; 
; Operation: cascade of S sections
;
;		 (b2_s*x[n-2] + b1_s*x[n-1] + b0_s*x[n])
;	y_s[n] = ---------------------------------------
;			(a2_s*y[n-2] + a1_s*y[n-1])
;
; Input:		
;	w0 = number of samples to generate (numSamps, N)
;	w1 = ptr to output samples (dstSamps, y)
;	w2 = ptr to input samples (srcSamps, x)
;	w3 = filter structure (IIRTransposedStruct, h)
;
; Return:
;	w0 = ptr to output samples (dstSamps, y)
;
; System resources usage:
;	{w0..w7}	used, not restored
;	{w8..w11}	saved, used, restored
;	 AccuA		used, not restored
;	 AccuB		used, not restored
;	 CORCON		saved, used, restored
;	 PSVPAG		saved, used, restored (if coeffs in program memory)
;
; DO and REPEAT instruction usage.
;	2 level DO instructions
;	no REPEAT intructions
;
; Program words (24-bit instructions):
;	48
;
; Cycles (including C-function call and return overheads):
;	35 + N*(11 + 11*S), or
;	38 + N*( 9 + 17*S) if coefficients in program memory,
; with S the number of second order sections.
;............................................................................

	.global	_IIRTransposed	; export
_IIRTransposed:

;............................................................................

	; Save working registers.
	push.d	w8				; {w8,w9} to TOS
	push.d	w10				; {w10,w11} to TOS

;............................................................................

	; Prepare CORCON for fractional computation.
	push	CORCON
	fractsetup	w8

;............................................................................

	; Prepare CORCON and PSVPAG for possible access of data
	; located in program memory, using the PSV.
	push	PSVPAG

;............................................................................

	push	w1				; save return value (dstSamps)

;............................................................................

	mov	[w3+oPSVpage],w10		; w10= coefficients page
	mov	#COEFFS_IN_DATA,w8		; w8 = COEFFS_IN_DATA
	cp	w8,w10				; w8 - w10
	bra	z,_noPSV			; if w10 = COEFFS_IN_DATA
						; no PSV management
						; else
	psvaccess	w8			; enable PSV bit in CORCON
	mov	w10,PSVPAG			; load PSVPAG with program
						; space page offset
_noPSV:

;............................................................................

	; Perpare to filter.
	mov	[w3+oNumSectionsLess1],w4	; w4 = number of sections - 1
	mov	[w3+oFinalShift],w9		; w9 = final shift
	dec	w0,w0				; w0 = N-1

;............................................................................

	; Perform filtering of all samples.
	do	w0,_endFilter		; {	; do (N-1)+1 times

	; Prepare biquadratic sections.
	mov	[w3+oCoeffs],w8			; w8 = base address of coeffs
	mov	[w3+oStates1],w10		; w10= base address of states 1
	mov	[w3+oStates2],w11		; w11= base address of states 2

	; Fetch next sample.
	mov	[w2++],w6			; w6 = x[n]
						;    = X[0] (intermediate)
						; w2-> x[n+1]
	; Fetch first coefficient and state 1.
	mov	[w8++],w5			; w5 = b0[0]
						; w8-> b1[m]
	lac	[w10],#1,a			; a  = del1[0]

	; Apply cascade of sections.
	do	w4,_endSections			; do (M-1)+1 times

	mac	w5*w6,a,[w8]+=2,w5		; a  = del1[m]
						;    + b0[m]*X[m]
						; w5 = b1[m]
						; w8-> a1[m]
	lac	[w11],#1,b			; b  = del2[m]
	sac.r	a,#-1,w7			; w7 = Y[m] (intermediate)
	mac	w5*w6,b,[w8]+=2,w5		; b  = del2[m]
						;    + b1[m]*X[m]
						; w5 = a1[m]
						; w8-> b2[m]
	mac	w5*w7,b,[w8]+=2,w5		; b  = del2[m]
						;    + b1[m]*X[n]
						;    + a1[m]*Y[m]
						; w5 = b2[m]
						; w8-> a2[m]
	sac.r	b,#-1,[w10++]			; update del1[m]
						; w10->del1[m+1]
	mpy	w5*w6,b,[w8]+=2,w5		; b  = b2[m]*X[n]
						; w5 = a2[m+1]
						; w8-> b0[m+1]
	sac.r	a,#-1,w6			; w6 = X[m+1] (intermediate)
	lac	[w10],#1,a			; a  = del1[m+1]
	mac	w5*w7,b,[w8]+=2,w5		; b  = b2[m]*X[n]
						;    + a2[m]*Y[m]
						; w5 = b0[m+1]
						; w8-> b1[m+1]
_endSections:
	sac.r	b,#-1,[w11++]			; update del2[m]
						; w10->del2[m+1]

	; Apply final shift.
	lac	w6,a				; a  = Y[M-1]
	sftac	a,w9				; restore section dB gain

_endFilter:
	; Round and store output.
	sac.r	a,[w1++]			; y[n] = rnd(a)
						; w1-> y[n+1]
;............................................................................

	pop	w0				; restore return value

;............................................................................

	; Restore PSVPAG and CORCON.
	pop	PSVPAG
	pop	CORCON

;............................................................................

	; Restore working registers.
	pop.d	w10				; {w10,w11} from TOS
	pop.d	w8				; {w8,w9} from TOS

;............................................................................

	return	

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

;############################################################################

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;
;
; _IIRTransposedInit: IIR Transposed Form II filter cascade initialization.
; 
; Operation:
;	filter->state1[n] = 0,		0 <= n <= filter->numSectionsLess1
;	filter->state2[n] = 0,		0 <= n <= filter->numSectionsLess1
;
; Input:		
;	w0 = filter structure (filter, h)
;
; Return:
;	(void)
;
; System resources usage:
;	{w0..w2}	used, not restored
;
; DO and REPEAT instruction usage.
;	1 level DO instructions
;	no REPEAT intructions
;
; Program words (24-bit instructions):
;	8
;
; Cycles (including C-function call and return overheads):
;	11 + 2*S,
; with S the number of second order sections.
;............................................................................

	.global _IIRTransposedInit

_IIRTransposedInit:
		
;............................................................................

	; Prepare operation.
	mov	[w0+oStates1],w1			; w1-> delayBase1[0]
	mov	[w0+oStates2],w2			; w2-> delayBase2[0]
	mov	[w0+oNumSectionsLess1],w0		; w0 = num sections -1

;............................................................................

	; Perform operation.
	do	w0,transposedInitLoop		; do num sections times
	clr	[w1++]				; delayBase1[m] = 0
						; w1-> delayBase1[m+1]
transposedInitLoop:		
	clr	[w2++]				; delayBase2[m] = 0
						; w2-> delayBase2[m+1]
		
;............................................................................

	return	

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

	.end

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
; OEF