6th_order_iir_call.s90

硬件设计资料,刚上网查找资料,是个新手,望大家提携了,

;****************************************************
; Function: 2nd order IIR filter
;
; Requirements: 
;	Implemented to be used with the ImageCraft compiler (6.24b).
;	For AVR devices with HW multiplier.
;
;	Coefficients should be scaled to 12 bit signed or less. The current
;	implementation uses 12 bit signed coefficients. The scaling used is
;	1024 since largest coefficient is -1.9216
;	The result of each filter run, y(n), should be scaled back according 
;	to the scaling used, to avoid a gain in the feedback part of the
;	filter. 	
;****************************************************
	NAME	assembly(16)
	PUBLIC	IIR6
	RSEG	CODE

;ZERO register (used to add carry flag)
#define ZERO  r2

#define ZL R30
#define SW_STACK Y

;Accumulator - 40bit
#define AC4 r13
#define AC3 r14
#define AC2 r15
#define AC1 r16
#define AC0 r17

;First argument passed in the function call: the address to the filter struct (16 bit address pointer)
#define FILTER_POINTER r16

;Second argument passed in the function call: the new data sample
#define NDATAL R18
#define NDATAH R19

;Data samples mul register (used in multiplication)
#define DATAL R20
#define DATAH R21

;Filter coefficient mul register (used in multiplication)
#define COEFL R22
#define COEFH R23

;Filter nodes offset relative to filter struct address
#define X1  0*2
#define X2  1*2
#define X3  2*2
#define X4  3*2
#define X5  4*2
#define X6  5*2

#define Y1  6*2
#define Y2  7*2
#define Y3  8*2
#define Y4  9*2
#define Y5  10*2
#define Y6  11*2

;Filter coefficient offset relative to filter struct address
#define B0  12*2
#define B1  13*2
#define B2  14*2
#define B3  15*2
#define B4  16*2
#define B5  17*2
#define B6  18*2

#define A1  19*2
#define A2  20*2
#define A3  21*2
#define A4  22*2
#define A5  23*2
#define A6  24*2

;**************************************************
;* MACROS
;**************************************************
STORE_REGISTERS MACRO
    st  -Y, AC4
    st  -Y, AC3
    st  -Y, AC2
    ENDM
    
RESTORE_REGISTERS MACRO
    ld  AC2, Y+
    ld  AC3, Y+
    ld  AC4, Y+
    ENDM

LOAD_NODE MACRO
	ldd	  DATAL,Z+\1			; Load low byte of node 
	ldd	  DATAH,Z+\1+1		; Load high byte of node
    ENDM

UPDATE_NODE MACRO
	std	  Z+\1, DATAL			; Update low byte of node to prepare next filter run
	std	  Z+\1+1, DATAH		; Update high byte of node to prepare next filter run
    ENDM

LOAD_COEF MACRO
	ldd	  COEFL,Z+\1			; Load low byte of node 
	ldd	  COEFH,Z+\1+1		; Load high byte of node
    ENDM

MUL_MOV_32 MACRO
	muls	COEFH, DATAH			; Signed multiply, coefficient high byte and data high byte
	movw    AC3, r0					; Copy result word into accumulator byte 2:3
    	
	mul	COEFL, DATAL		 	; Unsigned multiply, coefficient low byte and data low byte
	movw	AC1, r0					; Copy result word into accumulator byte 2:3

	mulsu	COEFH, DATAL		 	; Signed-unsigned multiply, coefficient high byte and data low byte
	sbc	AC3, ZERO				; Sign extention
	add	AC1, r0					; Add low byte of result to accumulator byte 1
	adc	AC2, r1					; Add with carry high byte of result to accumulator byte 2
	adc	AC3, ZERO				; Add carry to accumulator byte 3

	mulsu	DATAH, COEFL			; Signed-unsigned multiply, data high byte and coefficient low byte
	sbc	AC3, ZERO				; Sign extention
	add	AC1, r0					; Add low byte of result to accumulator byte 1
	adc	AC2, r1					; Add with carry high byte of result to accumulator byte 2
	adc	AC3, ZERO				; Add carry to accumulator byte 3
    ENDM

SMAC32:
;SMAC32 MACRO
	muls	COEFH, DATAH			; Signed multiply, coefficient high byte and data high byte
	add	AC2, r0					; Add low byte of result to accumulator byte 2
	adc	AC3, r1					; Add with carry high byte of result to accumulator byte 3

	mul	COEFL, DATAL			; Unsigned multiply, coefficient low byte and data low byte
	add	AC0, r0					; Add low byte of result to accumulator byte 0
	adc	AC1, r1					; Add with carry high byte of result to accumulator byte 1
	adc	AC2, ZERO				; Add carry to accumulator byte 2
	adc	AC3, ZERO				; Add carry to accumulator byte 3

	mulsu	COEFH, DATAL			; Signed-unsigned multiply, coefficient high byte and data low byte 
	sbc	AC3, ZERO				; Sign extention
	add	AC1, r0					; Add low byte of result to accumulator byte 1
	adc	AC2, r1					; Add with carry high byte of result to accumulator byte 2
	adc	AC3, ZERO				; Add carry to accumulator byte 3

	mulsu	DATAH, COEFL 			; Signed-unsigned multiply, data high byte and coefficient low byte
	sbc	AC3, ZERO				; Sign extention
	add	AC1, r0					; Add low byte of result to accumulator byte 1
	adc	AC2, r1					; Add with carry high byte of result to accumulator byte 2
	adc	AC3, ZERO				; Add carry to accumulator byte 3
	ret
;    ENDM

;*****************************************************
; FILTER takes a single samples ....
;*****************************************************
IIR6:
	STORE_REGISTERS			; Save registers that must be preserved accross function call
	clr	ZERO					; Clear ZERO register 
	movw	ZL, r16				; Move Struct pointer to the Z-pointer (r30:r31)
	
;B6*x[n-6]
	LOAD_COEF B6
	LOAD_NODE X6
	MUL_MOV_32

;B5*x[n-5]
	LOAD_COEF B5
	LOAD_NODE X5
	UPDATE_NODE X6
;	SMAC32
	rcall SMAC32
	
;B4*x[n-4]
	LOAD_COEF B4
	LOAD_NODE X4
	UPDATE_NODE X5
;	SMAC32
	rcall SMAC32
	
;B3*x[n-3]
	LOAD_COEF B3
	LOAD_NODE X3
	UPDATE_NODE X2
;	SMAC32
	rcall SMAC32
	
;B2*x[n-2]
	LOAD_COEF B2
	LOAD_NODE X2
	UPDATE_NODE X3
;	SMAC32
	rcall SMAC32
	
;B1*x[n-1]
	LOAD_COEF B1
	LOAD_NODE X1
	UPDATE_NODE X2
;	SMAC32
	rcall SMAC32
	
;B0*x[n]
	LOAD_COEF B0
	movw	DATAL, NDATAL		; Load new sample into data multiply register
	UPDATE_NODE X1
;	SMAC32
	rcall SMAC32

;A6*y[n-6]
	LOAD_COEF A6
	LOAD_NODE Y6
;	SMAC32
	rcall SMAC32

;A5*y[n-5]
	LOAD_COEF A5
	LOAD_NODE Y5
	UPDATE_NODE X6
;	SMAC32
	rcall SMAC32

;A4*y[n-4]
	LOAD_COEF A4
	LOAD_NODE Y4
	UPDATE_NODE X5
;    SMAC32
	rcall SMAC32

;A3*y[n-3]
	LOAD_COEF A3
	LOAD_NODE Y3
	UPDATE_NODE X4
;	SMAC32
	rcall SMAC32

;A2*y[n-2]
	LOAD_COEF A2
	LOAD_NODE Y2
	UPDATE_NODE X3
;	SMAC32
	rcall SMAC32

;A1*y[n-1]
	LOAD_COEF A1
	LOAD_NODE Y1
	UPDATE_NODE Y2
;	SMAC32
	rcall SMAC32

;Due to coefficient scaling (gain factor 2^10) the output requires "unscaling"
	asr AC2			; Scaling to gain factor 2^9
	ror	AC1			; --

	asr AC2			; Scaling to gain factor 2^8
	ror	AC1			; --
	
;Updating y[n-1] node
;(By copying result from AC1:2 instead of AC0:1 the gain factor becomes 1 (2^0))
	std	Z+Y1, AC1	; Update low byte of y[n-1] node to prepare next filter run 
	std	Z+Y1+1, AC2	; Update high byte of y[n-1] node to prepare next filter run 
	
;Restore registers and return (return value stored in accumulator AC2 and AC1, which are the return registers)
	RESTORE_REGISTERS	; Save registers that must be preserved accross function call
	ret
	END