;// ____________________________________________________________________
;//
;// Copyright (c) 2003, Andrew N. Sloss, Dominic Symes, Chris Wright
;// All rights reserved.
;// ____________________________________________________________________
;// 
;// NON-COMMERCIAL USE License
;// 
;// Redistribution and use in source and binary forms, with or without 
;// modification, are permitted provided that the following conditions 
;// are met: 
;//
;// 1. For NON-COMMERCIAL USE only.
;// 
;// 2. Redistributions of source code must retain the above copyright 
;//    notice, this list of conditions and the following disclaimer. 
;//
;// 3. Redistributions in binary form must reproduce the above 
;//    copyright notice, this list of conditions and the following 
;//    disclaimer in the documentation and/or other materials provided 
;//    with the distribution. 
;//
;// 4. All advertising materials mentioning features or use of this 
;//    software must display the following acknowledgement:
;//
;//    This product includes software developed by Andrew N. Sloss,
;//    Chris Wright and Dominic Symes. 
;//
;//  THIS SOFTWARE IS PROVIDED BY THE CONTRIBUTORS ``AS IS'' AND ANY 
;//  EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
;//  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 
;//  PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE CONTRIBUTORS BE 
;//  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, 
;//  OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 
;//  PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 
;//  OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 
;//  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR 
;//  TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT 
;//  OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY 
;//  OF SUCH DAMAGE. 
;//
;// If you have questions about this license or would like a different
;// license please email : andrew@sloss.net
;//
;// Section 8.5: FFT

        AREA    ch08_5, CODE, READONLY

        EXPORT  fft_16_arm9m
        EXPORT  fft_16_arm9e

;// Example 8.17
        
        ; complex load, x=[a], a+=offset
        MACRO
        C_LDR   $x, $a, $offset
        LDRSH   $x._i, [$a, #2]
        LDRSH   $x._r, [$a], $offset
        MEND
        
        ; complex store, [a]=x, a+=offset
        MACRO
        C_STR   $x, $a, $offset
        STRH    $x._i, [$a, #2]
        STRH    $x._r, [$a], $offset
        MEND        
        
        ; Four point complex Fast Fourier Transform
        ;
        ; (x0,x1,x2,y3)=DFT4(x0,x2>>s,x1>>s,x3>>s)/4
        ;
        ; x0 = (x0 +   (x2>>s) + (x1>>s) +   (x3>>s))/4
        ; x1 = (x0 - i*(x2>>s) - (x1>>s) + i*(x3>>s))/4
        ; x2 = (x0 -   (x2>>s) + (x1>>s) -   (x3>>s))/4
        ; y3 = (x0 + i*(x2>>s) - (x1>>s) - i*(x3>>s))/4
        ;
        MACRO
        C_FFT4  $s
        ; (x2,x3) = (x2+x3, x2-x3)
        ADD     x2_r, x2_r, x3_r
        ADD     x2_i, x2_i, x3_i
        SUB     x3_r, x2_r, x3_r, LSL#1
        SUB     x3_i, x2_i, x3_i, LSL#1
        ; (x0,x1) = (x0+(x1>>s), x0-(x1>>s))/4
        MOV     x0_r, x0_r, ASR#2
        MOV     x0_i, x0_i, ASR#2
        ADD     x0_r, x0_r, x1_r, ASR#(2+$s)
        ADD     x0_i, x0_i, x1_i, ASR#(2+$s)
        SUB     x1_r, x0_r, x1_r, ASR#(1+$s)
        SUB     x1_i, x0_i, x1_i, ASR#(1+$s)
        ; (x0,x2) = (x0+(x2>>s)/4, x0-(x2>>s)/4)
        ADD     x0_r, x0_r, x2_r, ASR#(2+$s)
        ADD     x0_i, x0_i, x2_i, ASR#(2+$s)
        SUB     x2_r, x0_r, x2_r, ASR#(1+$s)
        SUB     x2_i, x0_i, x2_i, ASR#(1+$s)
        ; (x1,y3) = (x1-i*(x3>>s)/4, x1+i*(x3>>s)/4)
        ADD     x1_r, x1_r, x3_i, ASR#(2+$s)
        SUB     x1_i, x1_i, x3_r, ASR#(2+$s)
        SUB     y3_r, x1_r, x3_i, ASR#(1+$s)
        ADD     y3_i, x1_i, x3_r, ASR#(1+$s)
        MEND
        
        ; Complex conjugate multiply a=(xr+i*xi)*(cr-i*ci)
        ;  x = xr + i*xi
        ;  w = (cr-ci) + i*ci
        MACRO
        C_MUL9m $a, $x, $w
        SUB     t1, $x._i, $x._r        ; (xi-xr)
        MUL     t0, t1, $w._i           ; (xi-xr)*ci
        ADD     t1, $w._r, $w._i, LSL#1 ; (cr+ci)
        MLA     $a._i, $x._i, $w._r, t0 ; xi*cr-xr*ci
        MLA     $a._r, $x._r, t1, t0    ; xr*cr+xi*ci
        MEND
        
y       RN 0    ; output complex array y[]
c       RN 0    ; coefficient array
x       RN 1    ; input complex array x[]
N       RN 2    ; number of samples (a power of 2)
S       RN 2    ; the number of blocks
R       RN 3    ; the number of samples in each block
x0_r    RN 4    ; data register (real part)
x0_i    RN 5    ; data register (imaginary part)
x1_r    RN 6
x1_i    RN 7
x2_r    RN 8
x2_i    RN 9
x3_r    RN 10
x3_i    RN 11
y3_r    RN x3_i
y3_i    RN x3_r
t0      RN 12   ; scratch register
t1      RN 14

        ; void fft_16_arm9m(short *y, short *x, unsigned int N)
fft_16_arm9m
        STMFD   sp!, {r4-r11, lr}
        MOV     t0, #0            ; bit reversed counter
first_stage_arm9m
        ; first stage load and bit reverse
        ADD     t1,  x, t0, LSL#2
        C_LDR   x0, t1, N
        C_LDR   x2, t1, N
        C_LDR   x1, t1, N
        C_LDR   x3, t1, N
        C_FFT4  0
        C_STR   x0, y, #4
        C_STR   x1, y, #4
        C_STR   x2, y, #4
        C_STR   y3, y, #4
        EOR     t0, t0, N, LSR#3  ; increment third bit
        TST     t0, N, LSR#3      ; from the top
        BNE     first_stage_arm9m
        EOR     t0, t0, N, LSR#4  ; increment fourth bit
        TST     t0, N, LSR#4      ; from the top
        BNE     first_stage_arm9m
        MOV     t1, N, LSR#5      ; increment fifth
bit_reversed_count_arm9m          ; bits downward
        EOR     t0, t0, t1
        TST     t0, t1
        BNE     first_stage_arm9m
        MOVS    t1, t1, LSR#1
        BNE     bit_reversed_count_arm9m
        ; finished the first stage
        SUB     x, y, N, LSL#2    ; x = working buffer
        MOV     R, #16
        MOVS    S, N, LSR#4
        LDMEQFD sp!, {r4-r11, pc}
        ADR     c, fft_table_arm9m
next_stage_arm9m
        ; S = the number of blocks
        ; R = the number of samples in each block
        STMFD   sp!, {x, S}
        ADD     t0, R, R, LSL#1
        ADD     x, x, t0
        SUB     S, S, #1<<16
next_block_arm9m
        ADD     S, S, R, LSL#(16-2)
next_butterfly_arm9m
        ; S=((number butterflies left-1)<<16)
        ;   + (number of blocks left)
        C_LDR   x0, x, -R
        C_LDR   x3, c, #4
        C_MUL9m x3, x0, x3
        C_LDR   x0, x, -R
        C_LDR   x2, c, #4
        C_MUL9m x2, x0, x2
        C_LDR   x0, x, -R
        C_LDR   x1, c, #4
        C_MUL9m x1, x0, x1
        C_LDR   x0, x, #0
        C_FFT4  14              ; coefficients are Q14
        C_STR   x0, x, R
        C_STR   x1, x, R
        C_STR   x2, x, R
        C_STR   y3, x, #4
        SUBS    S, S, #1<<16
        BGE     next_butterfly_arm9m
        ADD     t0, R, R, LSL#1
        ADD     x, x, t0
        SUB     S, S, #1
        MOVS    t1, S, LSL#16
        SUBNE   c, c, t0
        BNE     next_block_arm9m
        LDMFD   sp!, {x, S}
        MOV     R, R, LSL#2     ; quadrouple block size
        MOVS    S, S, LSR#2     ; quarter number of blocks
        BNE     next_stage_arm9m
        LDMFD   sp!, {r4-r11, pc}
        
fft_table_arm9m
        ; FFT twiddle table of triplets E(3t), E(t), E(2t)
        ; Where E(t)=(cos(t)-sin(t))+i*sin(t) at Q14
        ; N=16 t=2*PI*k/N for k=0,1,2,..,N/4-1
        DCW 0x4000,0x0000, 0x4000,0x0000, 0x4000,0x0000
        DCW 0xdd5d,0x3b21, 0x22a3,0x187e, 0x0000,0x2d41
        DCW 0xa57e,0x2d41, 0x0000,0x2d41, 0xc000,0x4000
        DCW 0xdd5d,0xe782, 0xdd5d,0x3b21, 0xa57e,0x2d41
        ; N=64 t=2*PI*k/N for k=0,1,2,..,N/4-1
        DCW 0x4000,0x0000, 0x4000,0x0000, 0x4000,0x0000
        DCW 0x2aaa,0x1294, 0x396b,0x0646, 0x3249,0x0c7c
        DCW 0x11a8,0x238e, 0x3249,0x0c7c, 0x22a3,0x187e
        DCW 0xf721,0x3179, 0x2aaa,0x1294, 0x11a8,0x238e
        DCW 0xdd5d,0x3b21, 0x22a3,0x187e, 0x0000,0x2d41
        DCW 0xc695,0x3fb1, 0x1a46,0x1e2b, 0xee58,0x3537
        DCW 0xb4be,0x3ec5, 0x11a8,0x238e, 0xdd5d,0x3b21
        DCW 0xa963,0x3871, 0x08df,0x289a, 0xcdb7,0x3ec5
        DCW 0xa57e,0x2d41, 0x0000,0x2d41, 0xc000,0x4000
        DCW 0xa963,0x1e2b, 0xf721,0x3179, 0xb4be,0x3ec5
        DCW 0xb4be,0x0c7c, 0xee58,0x3537, 0xac61,0x3b21
        DCW 0xc695,0xf9ba, 0xe5ba,0x3871, 0xa73b,0x3537
        DCW 0xdd5d,0xe782, 0xdd5d,0x3b21, 0xa57e,0x2d41
        DCW 0xf721,0xd766, 0xd556,0x3d3f, 0xa73b,0x238e
        DCW 0x11a8,0xcac9, 0xcdb7,0x3ec5, 0xac61,0x187e
        DCW 0x2aaa,0xc2c1, 0xc695,0x3fb1, 0xb4be,0x0c7c
        ; N=256 t=2*PI*k/N for k=0,1,2,..,N/4-1
        DCW 0x4000,0x0000, 0x4000,0x0000, 0x4000,0x0000
        DCW 0x3b1e,0x04b5, 0x3e69,0x0192, 0x3cc8,0x0324
        DCW 0x35eb,0x0964, 0x3cc8,0x0324, 0x396b,0x0646
        DCW 0x306c,0x0e06, 0x3b1e,0x04b5, 0x35eb,0x0964
        DCW 0x2aaa,0x1294, 0x396b,0x0646, 0x3249,0x0c7c
        DCW 0x24ae,0x1709, 0x37af,0x07d6, 0x2e88,0x0f8d
        DCW 0x1e7e,0x1b5d, 0x35eb,0x0964, 0x2aaa,0x1294
        DCW 0x1824,0x1f8c, 0x341e,0x0af1, 0x26b3,0x1590
        DCW 0x11a8,0x238e, 0x3249,0x0c7c, 0x22a3,0x187e
        DCW 0x0b14,0x2760, 0x306c,0x0e06, 0x1e7e,0x1b5d
        DCW 0x0471,0x2afb, 0x2e88,0x0f8d, 0x1a46,0x1e2b