fftbook.s

在ICCAVR 环境实现的fft,

	st -y,R16
	st -y,R9
	st -y,R8
	st -y,R7
	st -y,R6
	xcall empy32fs
	xcall sub32f
	xcall fp2int
	movw R30,R24
	std z+1,R17
	std z+0,R16
	.dbline 37
;     dataI[k+b]=TI+temp*sin(2*PI*p/16)-dataI[k+b]*cos(2*PI*p/16);
	ldi R16,<L3
	ldi R17,>L3
	xcall lpm32
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	lds R16,_p
	lds R17,_p+1
	xcall int2fp
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	xcall empy32fs
	ldi R16,<L4
	ldi R17,>L4
	xcall lpm32
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	xcall div32f
	xcall _sin
	movw R2,R16
	movw R4,R18
	ldi R16,<L3
	ldi R17,>L3
	xcall lpm32
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	lds R16,_p
	lds R17,_p+1
	xcall int2fp
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	xcall empy32fs
	ldi R16,<L4
	ldi R17,>L4
	xcall lpm32
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	xcall div32f
	push R2
	push R3
	push R4
	push R5
	xcall _cos
	pop R5
	pop R4
	pop R3
	pop R2
	movw R6,R16
	movw R8,R18
	ldi R16,<L5
	ldi R17,>L5
	xcall lpm32
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	lds R16,_TI
	lds R17,_TI+1
	lsr R17
	ror R16
	xcall int2fp
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	xcall empy32fs
	lds R16,_TI
	lds R17,_TI+1
	andi R16,1
	andi R17,0
	xcall int2fp
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	xcall add32fs
	ldi R16,<L5
	ldi R17,>L5
	xcall lpm32
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	lds R16,_temp
	lds R17,_temp+1
	lsr R17
	ror R16
	xcall int2fp
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	xcall empy32fs
	lds R16,_temp
	lds R17,_temp+1
	andi R16,1
	andi R17,0
	xcall int2fp
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	xcall add32fs
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	xcall empy32fs
	xcall add32fs
	lds R2,_b
	lds R3,_b+1
	lds R18,_k
	lds R19,_k+1
	add R18,R2
	adc R19,R3
	ldi R16,2
	ldi R17,0
	xcall empy16s
	movw R30,R16
	ldi R24,<_dataI
	ldi R25,>_dataI
	add R30,R24
	adc R31,R25
	ldd R16,z+0
	ldd R17,z+1
	xcall int2fp
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	st -y,R9
	st -y,R8
	st -y,R7
	st -y,R6
	xcall empy32fs
	xcall sub32f
	xcall fp2int
	movw R2,R16
	lds R4,_b
	lds R5,_b+1
	lds R18,_k
	lds R19,_k+1
	add R18,R4
	adc R19,R5
	ldi R16,2
	ldi R17,0
	xcall empy16s
	movw R30,R16
	ldi R24,<_dataI
	ldi R25,>_dataI
	add R30,R24
	adc R31,R25
	std z+1,R3
	std z+0,R2
	.dbline -2
L2:
	.dbline 0 ; func end
	ret
	.dbend
	.dbfunc e FFT _FFT fV
;          dataI -> R20,R21
;          dataR -> R22,R23
	.even
_FFT::
	xcall push_gset3
	movw R20,R18
	movw R22,R16
	.dbline -1
	.dbline 46
; 	
; 	/*dataR[k]=dataR[k]+dataR[k+b]*cos_tab[p]+dataI[k+b]*sin_tab[p];
;     dataI[k]=dataI[k]-dataR[k+b]*sin_tab[p]+dataI[k+b]*cos_tab[p];
;     dataR[k+b]=TR-dataR[k+b]*cos_tab[p]-dataI[k+b]*sin_tab[p];
;     dataI[k+b]=TI+temp*sin_tab[p]-dataI[k+b]*cos_tab[p];*/
; 
;  }
; void FFT(int dataR[],int dataI[])
; {
	.dbline 48
; // 以下是数据掉头功能
;   for(i = 0 ; i < 16 ; i++)
	clr R2
	clr R3
	sts _i+1,R3
	sts _i,R2
L7:
	.dbline 49
	.dbline 50
	xcall _CHANGE
	.dbline 51
L8:
	.dbline 48
	lds R24,_i
	lds R25,_i+1
	adiw R24,1
	sts _i+1,R25
	sts _i,R24
	.dbline 48
	cpi R24,16
	ldi R30,0
	cpc R25,R30
	brlt L7
	.dbline 52
;    { 
;     CHANGE ( ) ;
;    }
;   for(i=0;i<16;i++)
	clr R2
	clr R3
	sts _i+1,R3
	sts _i,R2
L11:
	.dbline 53
	.dbline 54
	lds R18,_i
	lds R19,_i+1
	ldi R16,2
	ldi R17,0
	xcall empy16s
	movw R30,R16
	add R30,R20
	adc R31,R21
	ldd R2,z+0
	ldd R3,z+1
	movw R30,R16
	add R30,R22
	adc R31,R23
	std z+1,R3
	std z+0,R2
	.dbline 54
	lds R18,_i
	lds R19,_i+1
	ldi R16,2
	ldi R17,0
	xcall empy16s
	movw R30,R16
	add R30,R20
	adc R31,R21
	clr R2
	clr R3
	std z+1,R3
	std z+0,R2
	.dbline 55
L12:
	.dbline 52
	lds R24,_i
	lds R25,_i+1
	adiw R24,1
	sts _i+1,R25
	sts _i,R24
	.dbline 52
	cpi R24,16
	ldi R30,0
	cpc R25,R30
	brlt L11
	.dbline 57
;    {
;     dataR[i]=dataI[i]; dataI[i]=0; 
;    }
; // FFT算法
;   for(L=1;L<=4;L++) 						// 第一层循环
	ldi R24,1
	ldi R25,0
	sts _L+1,R25
	sts _L,R24
L15:
	.dbline 58
;   {
	.dbline 59
;      b=1; i=L-1;
	ldi R24,1
	ldi R25,0
	sts _b+1,R25
	sts _b,R24
	.dbline 59
	lds R24,_L
	lds R25,_L+1
	sbiw R24,1
	sts _i+1,R25
	sts _i,R24
	xjmp L20
L19:
	.dbline 61
	.dbline 62
	lds R18,_b
	lds R19,_b+1
	ldi R16,2
	ldi R17,0
	xcall empy16s
	sts _b+1,R17
	sts _b,R16
	.dbline 62
	lds R24,_i
	lds R25,_i+1
	sbiw R24,1
	sts _i+1,R25
	sts _i,R24
	.dbline 63
L20:
	.dbline 60
;      while(i>0) 
	clr R2
	clr R3
	lds R4,_i
	lds R5,_i+1
	cp R2,R4
	cpc R3,R5
	brlt L19
	.dbline 64
;     {
;       b=b*2; i--;
;     } 
;        for(j=0;j<=b-1;j++)						 // 第二层循环
	sts _j+1,R3
	sts _j,R2
	xjmp L25
L22:
	.dbline 65
;          { 
	.dbline 66
;            p=1; i=4-L;
	ldi R24,1
	ldi R25,0
	sts _p+1,R25
	sts _p,R24
	.dbline 66
	lds R2,_L
	lds R3,_L+1
	ldi R24,4
	sub R24,R2
	sbc R25,R3
	sts _i+1,R25
	sts _i,R24
	xjmp L27
L26:
	.dbline 68
	.dbline 69
	lds R18,_p
	lds R19,_p+1
	ldi R16,2
	ldi R17,0
	xcall empy16s
	sts _p+1,R17
	sts _p,R16
	.dbline 69
	lds R24,_i
	lds R25,_i+1
	sbiw R24,1
	sts _i+1,R25
	sts _i,R24
	.dbline 70
L27:
	.dbline 67
;            while(i>0)					
	clr R2
	clr R3
	lds R4,_i
	lds R5,_i+1
	cp R2,R4
	cpc R3,R5
	brlt L26
	.dbline 71
;                {
; 			   p=p*2; i--;
; 			   }
;            p=p*j;
	lds R18,_j
	lds R19,_j+1
	lds R16,_p
	lds R17,_p+1
	xcall empy16s
	sts _p+1,R17
	sts _p,R16
	.dbline 72
;             for(k=j;k<16;k=k+2*b) 				// 第三层循环
	lds R2,_j
	lds R3,_j+1
	sts _k+1,R3
	sts _k,R2
	xjmp L32
L29:
	.dbline 73
	.dbline 74
	xcall _FFTT
	.dbline 75
L30:
	.dbline 72
	lds R18,_b
	lds R19,_b+1
	ldi R16,2
	ldi R17,0
	xcall empy16s
	lds R2,_k
	lds R3,_k+1
	add R2,R16
	adc R3,R17
	sts _k+1,R3
	sts _k,R2
L32:
	.dbline 72
	lds R24,_k
	lds R25,_k+1
	cpi R24,16
	ldi R30,0
	cpc R25,R30
	brlt L29
	.dbline 76
L23:
	.dbline 64
	lds R24,_j
	lds R25,_j+1
	adiw R24,1
	sts _j+1,R25
	sts _j,R24
L25:
	.dbline 64
	lds R24,_b
	lds R25,_b+1
	sbiw R24,1
	lds R2,_j
	lds R3,_j+1
	cp R24,R2
	cpc R25,R3
	brlt X0
	xjmp L22
X0:
	.dbline 77
L16:
	.dbline 57
	lds R24,_L
	lds R25,_L+1
	adiw R24,1
	sts _L+1,R25
	sts _L,R24
	.dbline 57
	ldi R24,4
	ldi R25,0
	lds R2,_L
	lds R3,_L+1
	cp R24,R2
	cpc R25,R3
	brlt X1
	xjmp L15
X1:
	.dbline 78
;               { 
;                 FFTT();
;               }
;           }
;   } 
;   for(i=0;i<32;i++) 							// 32次以下的谐波分析
	clr R2
	clr R3
	sts _i+1,R3
	sts _i,R2
L33:
	.dbline 79
;    {
	.dbline 80
;     w[i]=sqrt(dataR[i]*dataR[i]+dataI[i]*dataI[i]);
	lds R18,_i
	lds R19,_i+1
	ldi R16,2
	ldi R17,0
	xcall empy16s
	movw R30,R16
	add R30,R22
	adc R31,R23
	ldd R2,z+0
	ldd R3,z+1
	movw R30,R16
	add R30,R20
	adc R31,R21
	ldd R4,z+0
	ldd R5,z+1
	movw R18,R4
	movw R16,R4
	xcall empy16s
	movw R4,R16
	movw R18,R2
	movw R16,R2
	xcall empy16s
	add R16,R4
	adc R17,R5
	xcall int2fp
	xcall _sqrt
	movw R2,R16
	movw R4,R18
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	ldi R16,<L39
	ldi R17,>L39
	xcall lpm32
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	xcall cmp32f
	brlt L37
	st -y,R5
	st -y,R4
	st -y,R3
	st -y,R2
	ldi R16,<L39
	ldi R17,>L39
	xcall lpm32
	st -y,R19
	st -y,R18
	st -y,R17
	st -y,R16
	xcall sub32f
	xcall fp2int
	movw R24,R16
	subi R24,0  ; offset = 32768
	sbci R25,128
	movw R10,R24
	xjmp L38
L37:
	movw R16,R2
	movw R18,R4
	xcall fp2int
	movw R10,R16
L38:
	ldi R24,<_w
	ldi R25,>_w
	lds R30,_i
	lds R31,_i+1
	add R30,R24
	adc R31,R25
	std z+0,R10
	.dbline 81
	lds R6,_i
	lds R7,_i+1
	add R6,R24
	adc R7,R25
	ldi R17,64
	movw R30,R6
	ldd R16,z+0
	xcall div8u
	movw R30,R6
	std z+0,R16
	.dbline 82
L34:
	.dbline 78
	lds R24,_i
	lds R25,_i+1
	adiw R24,1
	sts _i+1,R25
	sts _i,R24
	.dbline 78
	cpi R24,32
	ldi R30,0
	cpc R25,R30
	brge X2
	xjmp L33
X2:
	.dbline 83
;     w[i]=w[i]/64;
;    }
;   w[0]=w[0]/2;
	lds R6,_w
	lsr R6
	sts _w,R6
	.dbline -2
L6:
	xcall pop_gset3
	.dbline 0 ; func end
	ret
	.dbsym r dataI 20 pI
	.dbsym r dataR 22 pI
	.dbend
	.dbfunc e main _main fV
	.even
_main::
	.dbline -1
	.dbline 87
; }  
; 
; void main ()
;   {
	.dbline 88
;     FFT(dataR,dataI);
	ldi R18,<_dataI
	ldi R19,>_dataI
	ldi R16,<_dataR
	ldi R17,>_dataR
	xcall _FFT
L41:
	.dbline 89
L42:
	.dbline 89
	xjmp L41
X3:
	.dbline -2
L40:
	.dbline 0 ; func end
	ret
	.dbend
	.area bss(ram, con, rel)
	.dbfile D:\HJN\Design\AVR\fft\fftbook.c
_dataI::
	.blkb 32
	.dbsym e dataI _dataI A[32:16]I
_temp::
	.blkb 2
	.dbsym e temp _temp i
_TI::
	.blkb 2
	.dbsym e TI _TI i
_TR::
	.blkb 2
	.dbsym e TR _TR i
_w::
	.blkb 32
	.dbsym e w _w A[32:32]c
_p::
	.blkb 2
	.dbsym e p _p I
_b::
	.blkb 2
	.dbsym e b _b I
_k::
	.blkb 2
	.dbsym e k _k I
_j::
	.blkb 2
	.dbsym e j _j I
_i::
	.blkb 2
	.dbsym e i _i I
_L::
	.blkb 2
	.dbsym e L _L I
_x6::
	.blkb 2
	.dbsym e x6 _x6 i
_x5::
	.blkb 2
	.dbsym e x5 _x5 i
_x4::
	.blkb 2
	.dbsym e x4 _x4 i
_x3::
	.blkb 2
	.dbsym e x3 _x3 i
_x2::
	.blkb 2
	.dbsym e x2 _x2 i
_x1::
	.blkb 2
	.dbsym e x1 _x1 i
_x0::
	.blkb 2
	.dbsym e x0 _x0 i
	.area lit(rom, con, rel)
L39:
	.word 0x0,0x4700
L5:
	.word 0x0,0x4000
L4:
	.word 0x0,0x4180
L3:
	.word 0xfda,0x40c9