#ifdef __AVR__ ;-----------------------------------------------------------------------------; ; Fixed-point FFT routines for megaAVRs (C)ChaN, 2005 ;-----------------------------------------------------------------------------; ; * This program is opened under license policy of following trems. ; ; Copyright (C) 2005, ChaN, all right reserved. ; ; * This program is a free software and there is NO WARRANTY. ; * No restriction on use. You can use, modify and redistribute it for ; personal, non-profit or commercial use UNDER YOUR RESPONSIBILITY. ; * Redistributions of source code must retain the above copyright notice. ; ;-----------------------------------------------------------------------------; ; ; void fft_input (const int16_t *array_src, complex_t *array_bfly); ; void fft_execute (complex_t *array_bfly); ; void fft_output (complex_t *array_bfly, uint16_t *array_dst); ; ; : Wave form to be processed. ; : Complex array for butterfly operations. ; : Spectrum output buffer. ; ; These functions must be called in sequence to do a DFT in FFT algorithm. ; fft_input() fills the complex array with a wave form to prepare butterfly ; operations. A hamming window is applied at the same time. ; fft_execute() executes the butterfly operations. ; fft_output() re-orders the results, converts the complex spectrum into ; scalar spectrum and output it in linear scale. ; ;----------------------------------------------------------------------------; ; THIS CODE HAS BEEN MODIFIED FROM ITS ORIGINAL FORM - it is pared down ; to handle 64 points only, for the Circuit Playground microphone library. ; Original FFFT code can be found on GitHub. ;----------------------------------------------------------------------------; .list #define FFT_N 64 #define FFT_B 6 #define T0L r0 #define T0H r1 #define T2L r2 #define T2H r3 #define T4L r4 #define T4H r5 #define T6L r6 #define T6H r7 #define T8L r8 #define T8H r9 #define T10L r10 #define T10H r11 #define T12L r12 #define T12H r13 #define T14L r14 #define T14H r15 #define AL r16 #define AH r17 #define BL r18 #define BH r19 #define CL r20 #define CH r21 #define DL r22 #define DH r23 #define EL r24 #define EH r25 #define XL r26 #define XH r27 #define YL r28 #define YH r29 #define ZL r30 #define ZH r31 .macro ldiw dh,dl, abs ldi \dl, lo8(\abs) ldi \dh, hi8(\abs) .endm .macro subiw dh,dl, abs subi \dl, lo8(\abs) sbci \dh, hi8(\abs) .endm .macro addw dh,dl, sh,sl add \dl, \sl adc \dh, \sh .endm .macro addd d3,d2,d1,d0, s3,s2,s1,s0 add \d0, \s0 adc \d1, \s1 adc \d2, \s2 adc \d3, \s3 .endm .macro subw dh,dl, sh,sl sub \dl, \sl sbc \dh, \sh .endm .macro subd d3,d2,d1,d0, s3,s2,s1,s0 sub \d0, \s0 sbc \d1, \s1 sbc \d2, \s2 sbc \d3, \s3 .endm .macro lddw dh,dl, src ldd \dl, \src ldd \dh, \src+1 .endm .macro ldw dh,dl, src ld \dl, \src ld \dh, \src .endm .macro stw dst, sh,sl st \dst, \sl st \dst, \sh .endm .macro clrw dh, dl clr \dh clr \dl .endm .macro lsrw dh, dl lsr \dh ror \dl .endm .macro asrw dh, dl asr \dh ror \dl .endm .macro lslw dh, dl lsl \dl rol \dh .endm .macro pushw dh, dl push \dh push \dl .endm .macro popw dh, dl pop \dl pop \dh .endm .macro lpmw dh,dl, src lpm \dl, \src lpm \dh, \src .endm .macro rjne lbl breq 99f rjmp \lbl 99: .endm .macro FMULS16 d3,d2,d1,d0 ,s1h,s1l, s2h,s2l ;Fractional Multiply (19clk) fmuls \s1h, \s2h movw \d2, T0L fmul \s1l, \s2l movw \d0, T0L adc \d2, EH ;EH: zero reg. fmulsu \s1h, \s2l sbc \d3, EH add \d1, T0L adc \d2, T0H adc \d3, EH fmulsu \s2h, \s1l sbc \d3, EH add \d1, T0L adc \d2, T0H adc \d3, EH .endm .macro SQRT32 ; 32bit square root (526..542clk) clr T6L clr T6H clr T8L clr T8H ldi BL, 1 ldi BH, 0 clr CL clr CH ldi DH, 16 90: lsl T2L rol T2H rol T4L rol T4H rol T6L rol T6H rol T8L rol T8H lsl T2L rol T2H rol T4L rol T4H rol T6L rol T6H rol T8L rol T8H brpl 91f add T6L, BL adc T6H, BH adc T8L, CL adc T8H, CH rjmp 92f 91: sub T6L, BL sbc T6H, BH sbc T8L, CL sbc T8H, CH 92: lsl BL rol BH rol CL andi BL, 0b11111000 ori BL, 0b00000101 sbrc T8H, 7 subi BL, 2 dec DH brne 90b lsr CL ror BH ror BL lsr CL ror BH ror BL .endm ;----------------------------------------------------------------------------; ; Constant Tables .global tbl_window tbl_window: ; tbl_window[] = ... (This is a Hamming window) .dc.w 2621, 2693, 2910, 3270, 3768, 4401, 5161, 6042, 7036, 8132, 9320, 10588, 11926, 13318, 14753, 16216 .dc.w 17694, 19171, 20634, 22069, 23462, 24799, 26068, 27256, 28352, 29345, 30226, 30987, 31619, 32117, 32477, 32694 .dc.w 32766, 32694, 32477, 32117, 31619, 30987, 30226, 29345, 28352, 27256, 26068, 24799, 23462, 22069, 20634, 19171 .dc.w 17694, 16216, 14753, 13318, 11926, 10588, 9320, 8132, 7036, 6042, 5161, 4401, 3768, 3270, 2910, 2693 tbl_cos_sin: ; Table of {cos(x),sin(x)}, (0 <= x < pi, in FFT_N/2 steps) .dc.w 32767, 0, 32609, 3211, 32137, 6392, 31356, 9511, 30272, 12539, 28897, 15446, 27244, 18204, 25329, 20787 .dc.w 23169, 23169, 20787, 25329, 18204, 27244, 15446, 28897, 12539, 30272, 9511, 31356, 6392, 32137, 3211, 32609 .dc.w 0, 32766, -3211, 32609, -6392, 32137, -9511, 31356, -12539, 30272, -15446, 28897, -18204, 27244, -20787, 25329 .dc.w -23169, 23169, -25329, 20787, -27244, 18204, -28897, 15446, -30272, 12539, -31356, 9511, -32137, 6392, -32609, 3211 tbl_bitrev: ; tbl_bitrev[] = ... .dc.w 0*4, 32*4, 16*4, 48*4, 8*4, 40*4, 24*4, 56*4, 4*4, 36*4, 20*4, 52*4, 12*4, 44*4, 28*4, 60*4 .dc.w 2*4, 34*4, 18*4, 50*4, 10*4, 42*4, 26*4, 58*4, 6*4, 38*4, 22*4, 54*4, 14*4, 46*4, 30*4, 62*4 ;----------------------------------------------------------------------------; .global fft_input .func fft_input fft_input: pushw T2H,T2L pushw AH,AL pushw YH,YL movw XL, EL ;X = array_src; movw YL, DL ;Y = array_bfly; clr EH ;Zero ldiw ZH,ZL, tbl_window ;Z = &tbl_window[0]; ldiw AH,AL, FFT_N ;A = FFT_N; 1: lpmw BH,BL, Z+ ;B = *Z++; (window) ldw CH,CL, X+ ;C = *X++; (I-axis) FMULS16 DH,DL,T2H,T2L, BH,BL, CH,CL ;D = B * C; stw Y+, DH,DL ;*Y++ = D; stw Y+, DH,DL ;*Y++ = D; subiw AH,AL, 1 ;while(--A) brne 1b ;/ popw YH,YL popw AH,AL popw T2H,T2L clr r1 ret .endfunc ;----------------------------------------------------------------------------; .global fft_execute .func fft_execute fft_execute: pushw T2H,T2L pushw T4H,T4L pushw T6H,T6L pushw T8H,T8L pushw T10H,T10L pushw T12H,T12L pushw T14H,T14L pushw AH,AL pushw YH,YL movw ZL, EL ;Z = array_bfly; ldiw EH,EL, 1 ;E = 1; ldiw XH,XL, FFT_N/2 ;X = FFT_N/2; 1: ldi AL, 4 ;T12 = E; (angular speed) mul EL, AL ; movw T12L, T0L ; mul EH, AL ; add T12H, T0L ;/ movw T14L, EL ;T14 = E; pushw EH,EL movw YL, ZL ;Z = &array_bfly[0]; mul XL, AL ;Y = &array_bfly[X]; addw YH,YL, T0H,T0L ; mul XH, AL ; add YH, T0L ;/ pushw ZH,ZL 2: clrw T10H,T10L ;T10 = 0 (angle) clr EH ;Zero reg. 3: lddw AH,AL, Z+0 ;A = *Z - *Y; *Z++ += *Y; asrw AH,AL ; lddw DH,DL, Y+0 ; asrw DH,DL ; movw CL, AL ; subw AH,AL, DH,DL ; addw CH,CL, DH,DL ; stw Z+, CH,CL ;/ lddw BH,BL, Z+0 ;B = *Z - *Y; *Z++ += *Y; asrw BH,BL ; lddw DH,DL, Y+2 ; asrw DH,DL ; movw CL, BL ; subw BH,BL, DH,DL ; addw CH,CL, DH,DL ; stw Z+, CH,CL ;/ movw T0L, ZL ldiw ZH,ZL, tbl_cos_sin ;C = cos(T10); D = sin(T10); addw ZH,ZL, T10H,T10L ; lpmw CH,CL, Z+ ; lpmw DH,DL, Z+ ;/ movw ZL, T0L FMULS16 T4H,T4L,T2H,T2L, AH,AL, CH,CL ;*Y++ = A * C + B * D; FMULS16 T8H,T8L,T6H,T6L, BH,BL, DH,DL ; addd T4H,T4L,T2H,T2L, T8H,T8L,T6H,T6L; stw Y+, T4H,T4L ;/ FMULS16 T4H,T4L,T2H,T2L, BH,BL, CH,CL ;*Y++ = B * C - A * D; FMULS16 T8H,T8L,T6H,T6L, AH,AL, DH,DL ; subd T4H,T4L,T2H,T2L, T8H,T8L,T6H,T6L; stw Y+, T4H,T4L ;/ addw T10H,T10L, T12H,T12L ;T10 += T12; (next angle) #if FFT_N >= 128 sbrs T10H, FFT_B - 7 ;while(T10 < pi) #else sbrs T10L, FFT_B + 1 #endif rjmp 3b ;/ ldi AL, 4 ;Y += X; Z += X; (skip split segment) mul XL, AL addw YH,YL, T0H,T0L ; addw ZH,ZL, T0H,T0L ; mul XH, AL ; add YH, T0L ; add ZH, T0L ;/ ldi EL, 1 ;while(--T14) subw T14H,T14L, EH,EL ; rjne 2b ;/ popw ZH,ZL popw EH,EL lslw EH,EL ;E *= 2; lsrw XH,XL ;while(X /= 2) adiw XL, 0 ; rjne 1b ;/ popw YH,YL popw AH,AL popw T14H,T14L popw T12H,T12L popw T10H,T10L popw T8H,T8L popw T6H,T6L popw T4H,T4L popw T2H,T2L ; clr r1 ret .endfunc ;----------------------------------------------------------------------------; .global fft_output .func fft_output fft_output: pushw T2H,T2L pushw T4H,T4L pushw T6H,T6L pushw T8H,T8L pushw T10H,T10L pushw AH,AL pushw YH,YL movw T10L, EL ;T10 = array_bfly; movw YL, DL ;Y = array_output; ldiw ZH,ZL, tbl_bitrev ;Z = tbl_bitrev; clr EH ;Zero ldiw AH,AL, FFT_N / 2 ;A = FFT_N / 2; (plus only) 1: lpmw XH,XL, Z+ ;X = *Z++; addw XH,XL, T10H,T10L ;X += array_bfly; ldw BH,BL, X+ ;B = *X++; ldw CH,CL, X+ ;C = *X++; FMULS16 T4H,T4L,T2H,T2L, BH,BL, BH,BL ;T4:T2 = B * B; FMULS16 T8H,T8L,T6H,T6L, CH,CL, CH,CL ;T8:T6 = C * C; addd T4H,T4L,T2H,T2L, T8H,T8L,T6H,T6L;T4:T2 += T8:T6; SQRT32 ;B = sqrt(T4:T2); stw Y+, BH,BL ;*Y++ = B; subiw AH,AL, 1 ;while(--A) rjne 1b ;/ popw YH,YL popw AH,AL popw T10H,T10L popw T8H,T8L popw T6H,T6L popw T4H,T4L popw T2H,T2L clr r1 ret .endfunc ;----------------------------------------------------------------------------; .global fmuls_f .func fmuls_f fmuls_f: movw CL, EL ;C = E; clr EH ;Zero FMULS16 ZH,ZL,XH,XL, CH,CL, DH,DL ;Z:X = C * D; movw EL, ZL clr r1 ret .endfunc #endif // __AVR__