0
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
1 /*
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
2 Copyright (c) 2003-2010, Mark Borgerding
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
3
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
4 All rights reserved.
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
5
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
6 Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
7
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
8 * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
9 * 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.
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
10 * Neither the author nor the names of any contributors may be used to endorse or promote products derived from this software without specific prior written permission.
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
11
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
12 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND 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 COPYRIGHT OWNER OR 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.
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
13 */
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
14
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
15
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
16 #include "_peck_fft_guts.h"
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
17 /* The guts header contains all the multiplication and addition macros that are defined for
|
1
|
18 * fixed or floating point complex numbers. It also delares the kf_ internal functions.
|
0
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
19 */
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
20
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
21 static void kf_bfly2(
|
5
|
22 peck_fft_cpx *Fout,
|
4
|
23 const size_t fstride,
|
|
24 const peck_fft_cfg st,
|
|
25 int m) {
|
0
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
26
|
7
|
27 // printf("kf_bfly2, %d\n", fstride);
|
0
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
28
|
5
|
29 peck_fft_cpx *Fout2;
|
|
30 peck_fft_cpx *tw1 = st->twiddles;
|
0
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
31 peck_fft_cpx t;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
32 Fout2 = Fout + m;
|
1
|
33 do {
|
|
34 C_MUL(t, *Fout2, *tw1);
|
0
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
35 tw1 += fstride;
|
1
|
36 C_SUB(*Fout2, *Fout, t);
|
|
37 C_ADDTO(*Fout, t);
|
0
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
38 ++Fout2;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
39 ++Fout;
|
1
|
40 } while (--m);
|
0
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
41 }
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
42
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
43 static void kf_bfly4(
|
4
|
44 peck_fft_cpx * Fout,
|
|
45 const size_t fstride,
|
|
46 const peck_fft_cfg st,
|
|
47 const size_t m) {
|
|
48
|
0
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
49 peck_fft_cpx *tw1,*tw2,*tw3;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
50 peck_fft_cpx scratch[6];
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
51 size_t k=m;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
52 const size_t m2=2*m;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
53 const size_t m3=3*m;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
54
|
7
|
55 // printf("kf_bfly4, %d\n", fstride);
|
0
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
56
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
57 tw3 = tw2 = tw1 = st->twiddles;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
58
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
59 do {
|
1
|
60 C_MUL(scratch[0], Fout[m], *tw1);
|
|
61 C_MUL(scratch[1], Fout[m2], *tw2);
|
|
62 C_MUL(scratch[2], Fout[m3], *tw3);
|
|
63
|
|
64 C_SUB(scratch[5], *Fout, scratch[1]);
|
0
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
65 C_ADDTO(*Fout, scratch[1]);
|
1
|
66 C_ADD(scratch[3], scratch[0], scratch[2]);
|
|
67 C_SUB(scratch[4], scratch[0], scratch[2]);
|
|
68 C_SUB(Fout[m2], *Fout, scratch[3]);
|
0
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
69 tw1 += fstride;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
70 tw2 += fstride*2;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
71 tw3 += fstride*3;
|
1
|
72 C_ADDTO(*Fout, scratch[3]);
|
0
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
73
|
1
|
74 if (st->inverse) {
|
0
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
75 Fout[m].r = scratch[5].r - scratch[4].i;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
76 Fout[m].i = scratch[5].i + scratch[4].r;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
77 Fout[m3].r = scratch[5].r + scratch[4].i;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
78 Fout[m3].i = scratch[5].i - scratch[4].r;
|
1
|
79 } else {
|
0
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
80 Fout[m].r = scratch[5].r + scratch[4].i;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
81 Fout[m].i = scratch[5].i - scratch[4].r;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
82 Fout[m3].r = scratch[5].r - scratch[4].i;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
83 Fout[m3].i = scratch[5].i + scratch[4].r;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
84 }
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
85 ++Fout;
|
1
|
86 } while (--k);
|
0
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
87 }
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
88
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
89 static void kf_bfly3(
|
4
|
90 peck_fft_cpx * Fout,
|
|
91 const size_t fstride,
|
|
92 const peck_fft_cfg st,
|
|
93 size_t m) {
|
|
94
|
0
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
95 size_t k=m;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
96 const size_t m2 = 2*m;
|
1
|
97 peck_fft_cpx *tw1, *tw2;
|
0
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
98 peck_fft_cpx scratch[5];
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
99 peck_fft_cpx epi3;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
100 epi3 = st->twiddles[fstride*m];
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
101
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
102 printf("kf_bfly3\n");
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
103
|
5
|
104 tw1 = tw2 = st->twiddles;
|
0
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
105
|
1
|
106 do {
|
0
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
107 C_MUL(scratch[1],Fout[m] , *tw1);
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
108 C_MUL(scratch[2],Fout[m2] , *tw2);
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
109
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
110 C_ADD(scratch[3],scratch[1],scratch[2]);
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
111 C_SUB(scratch[0],scratch[1],scratch[2]);
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
112 tw1 += fstride;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
113 tw2 += fstride*2;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
114
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
115 Fout[m].r = Fout->r - HALF_OF(scratch[3].r);
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
116 Fout[m].i = Fout->i - HALF_OF(scratch[3].i);
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
117
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
118 C_MULBYSCALAR( scratch[0] , epi3.i );
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
119
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
120 C_ADDTO(*Fout,scratch[3]);
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
121
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
122 Fout[m2].r = Fout[m].r + scratch[0].i;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
123 Fout[m2].i = Fout[m].i - scratch[0].r;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
124
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
125 Fout[m].r -= scratch[0].i;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
126 Fout[m].i += scratch[0].r;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
127
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
128 ++Fout;
|
1
|
129 } while (--k);
|
0
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
130 }
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
131
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
132 static void kf_bfly5(
|
4
|
133 peck_fft_cpx * Fout,
|
|
134 const size_t fstride,
|
|
135 const peck_fft_cfg st,
|
|
136 int m
|
|
137 ) {
|
0
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
138 peck_fft_cpx *Fout0,*Fout1,*Fout2,*Fout3,*Fout4;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
139 int u;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
140 peck_fft_cpx scratch[13];
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
141 peck_fft_cpx * twiddles = st->twiddles;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
142 peck_fft_cpx *tw;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
143 peck_fft_cpx ya,yb;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
144 ya = twiddles[fstride*m];
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
145 yb = twiddles[fstride*2*m];
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
146
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
147 printf("kf_bfly5\n");
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
148
|
5
|
149 Fout0 = Fout;
|
|
150 Fout1 = Fout0 + m;
|
|
151 Fout2 = Fout0 + 2*m;
|
|
152 Fout3 = Fout0 + 3*m;
|
|
153 Fout4 = Fout0 + 4*m;
|
0
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
154
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
155 tw=st->twiddles;
|
4
|
156 for (u = 0; u < m; ++u) {
|
0
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
157 scratch[0] = *Fout0;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
158
|
6
|
159 C_MUL(scratch[1], *Fout1, tw[u*fstride]);
|
|
160 C_MUL(scratch[2], *Fout2, tw[2*u*fstride]);
|
|
161 C_MUL(scratch[3], *Fout3, tw[3*u*fstride]);
|
|
162 C_MUL(scratch[4], *Fout4, tw[4*u*fstride]);
|
0
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
163
|
6
|
164 C_ADD(scratch[7], scratch[1], scratch[4]);
|
|
165 C_SUB(scratch[10], scratch[1], scratch[4]);
|
|
166 C_ADD(scratch[8], scratch[2], scratch[3]);
|
|
167 C_SUB(scratch[9], scratch[2], scratch[3]);
|
0
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
168
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
169 Fout0->r += scratch[7].r + scratch[8].r;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
170 Fout0->i += scratch[7].i + scratch[8].i;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
171
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
172 scratch[5].r = scratch[0].r + S_MUL(scratch[7].r,ya.r) + S_MUL(scratch[8].r,yb.r);
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
173 scratch[5].i = scratch[0].i + S_MUL(scratch[7].i,ya.r) + S_MUL(scratch[8].i,yb.r);
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
174
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
175 scratch[6].r = S_MUL(scratch[10].i,ya.i) + S_MUL(scratch[9].i,yb.i);
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
176 scratch[6].i = -S_MUL(scratch[10].r,ya.i) - S_MUL(scratch[9].r,yb.i);
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
177
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
178 C_SUB(*Fout1,scratch[5],scratch[6]);
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
179 C_ADD(*Fout4,scratch[5],scratch[6]);
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
180
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
181 scratch[11].r = scratch[0].r + S_MUL(scratch[7].r,yb.r) + S_MUL(scratch[8].r,ya.r);
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
182 scratch[11].i = scratch[0].i + S_MUL(scratch[7].i,yb.r) + S_MUL(scratch[8].i,ya.r);
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
183 scratch[12].r = - S_MUL(scratch[10].i,yb.i) + S_MUL(scratch[9].i,ya.i);
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
184 scratch[12].i = S_MUL(scratch[10].r,yb.i) - S_MUL(scratch[9].r,ya.i);
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
185
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
186 C_ADD(*Fout2,scratch[11],scratch[12]);
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
187 C_SUB(*Fout3,scratch[11],scratch[12]);
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
188
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
189 ++Fout0;++Fout1;++Fout2;++Fout3;++Fout4;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
190 }
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
191 }
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
192
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
193 /* perform the butterfly for one stage of a mixed radix FFT */
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
194 static void kf_bfly_generic(
|
4
|
195 peck_fft_cpx * Fout,
|
|
196 const size_t fstride,
|
|
197 const peck_fft_cfg st,
|
|
198 int m,
|
|
199 int p) {
|
|
200
|
0
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
201 int u,k,q1,q;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
202 peck_fft_cpx * twiddles = st->twiddles;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
203 peck_fft_cpx t;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
204 int Norig = st->nfft;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
205
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
206 printf("kf_bfly_generic\n");
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
207
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
208 peck_fft_cpx * scratch = (peck_fft_cpx*)PECK_FFT_TMP_ALLOC(sizeof(peck_fft_cpx)*p);
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
209
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
210 for ( u=0; u<m; ++u ) {
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
211 k=u;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
212 for (q1 = 0; q1 < p; ++q1) {
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
213 scratch[q1] = Fout[k];
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
214 k += m;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
215 }
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
216
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
217 k=u;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
218 for ( q1=0 ; q1<p ; ++q1 ) {
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
219 int twidx=0;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
220 Fout[ k ] = scratch[0];
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
221 for (q=1;q<p;++q ) {
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
222 twidx += fstride * k;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
223 if (twidx>=Norig) twidx-=Norig;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
224 C_MUL(t,scratch[q] , twiddles[twidx] );
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
225 C_ADDTO( Fout[ k ] ,t);
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
226 }
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
227 k += m;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
228 }
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
229 }
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
230 PECK_FFT_TMP_FREE(scratch);
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
231 }
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
232
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
233 static void kf_work(
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
234 peck_fft_cpx * Fout,
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
235 const peck_fft_cpx * f,
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
236 const size_t fstride,
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
237 int *factors,
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
238 const peck_fft_cfg st) {
|
5
|
239
|
0
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
240 peck_fft_cpx *Fout_beg = Fout;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
241 const int p = *factors++; /* the radix */
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
242 const int m = *factors++; /* stage's FFT length / p */
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
243 const peck_fft_cpx *Fout_end = Fout + p*m;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
244
|
7
|
245 // printf("kf_work, %d\n", fstride);
|
0
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
246
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
247 if (m == 1) {
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
248 do {
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
249 *Fout = *f;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
250 f += fstride;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
251 } while (++Fout != Fout_end);
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
252 } else {
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
253 do {
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
254 // recursive call:
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
255 // DFT of size m*p performed by doing
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
256 // p instances of smaller DFTs of size m,
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
257 // each one takes a decimated version of the input
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
258 kf_work(Fout, f, fstride*p, factors, st);
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
259 f += fstride;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
260 } while ((Fout += m) != Fout_end);
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
261 }
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
262
|
5
|
263 Fout = Fout_beg;
|
0
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
264
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
265 // recombine the p smaller DFTs
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
266 switch (p) {
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
267 case 2: kf_bfly2(Fout, fstride, st, m); break;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
268 case 3: kf_bfly3(Fout, fstride, st, m); break;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
269 case 4: kf_bfly4(Fout, fstride, st, m); break;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
270 case 5: kf_bfly5(Fout, fstride, st, m); break;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
271 default: kf_bfly_generic(Fout, fstride, st, m, p); break;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
272 }
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
273 }
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
274
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
275 /*
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
276 * facbuf is populated by p1, m1, p2, m2, ...
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
277 * where
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
278 * p[i] * m[i] = m[i-1]
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
279 * m0 = n
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
280 */
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
281 static void kf_factor(int n, int * facbuf) {
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
282 int p = 4;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
283 float floor_sqrt;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
284 floor_sqrt = floorf(sqrtf(n));
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
285
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
286 /* factor out powers of 4, powers of 2, then any remaining primes */
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
287 do {
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
288 while (n % p) {
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
289 switch (p) {
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
290 case 4: p = 2; break;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
291 case 2: p = 3; break;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
292 default: p += 2; break;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
293 }
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
294 if (p > floor_sqrt)
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
295 p = n; /* no more factors, skip to end */
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
296 }
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
297 n /= p;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
298 *facbuf++ = p;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
299 *facbuf++ = n;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
300 } while (n > 1);
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
301 }
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
302
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
303 /*
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
304 * User-callable function to allocate all necessary storage space for the fft.
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
305 * The return value is a contiguous block of memory, allocated with malloc. As such,
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
306 * it can be freed with free(), rather than a peck_fft-specific function.
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
307 */
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
308 peck_fft_cfg peck_fft_alloc(int nfft, int inverse_fft, void * mem, size_t * lenmem) {
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
309 peck_fft_cfg st = NULL;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
310 size_t memneeded = sizeof(struct peck_fft_state)
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
311 + sizeof(peck_fft_cpx)*(nfft-1); /* twiddle factors */
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
312
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
313 if (lenmem == NULL) {
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
314 st = ( peck_fft_cfg)PECK_FFT_MALLOC(memneeded);
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
315 } else {
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
316 if (mem != NULL && *lenmem >= memneeded)
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
317 st = (peck_fft_cfg)mem;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
318 *lenmem = memneeded;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
319 }
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
320
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
321 if (st) {
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
322 int i;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
323 st->nfft=nfft;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
324 st->inverse = inverse_fft;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
325
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
326 for (i = 0; i < nfft; ++i) {
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
327 const float pi = 3.14159265359f;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
328 float phase = -2*pi*i / nfft;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
329 if (st->inverse)
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
330 phase *= -1;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
331 kf_cexp(st->twiddles+i, phase);
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
332 }
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
333
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
334 kf_factor(nfft, st->factors);
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
335 }
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
336 return st;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
337 }
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
338
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
339 void peck_fft(peck_fft_cfg cfg, const peck_fft_cpx *fin, peck_fft_cpx *fout) {
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
340 kf_work(fout, fin, 1, cfg->factors, cfg);
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
341 }
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
342
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
343 void peck_fft_cleanup(void) {
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
344 /* nothing needed any more */
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
345 }
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
346
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
347 int peck_fft_next_fast_size(int n) {
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
348 while (1) {
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
349 int m = n;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
350 while ((m % 2) == 0) m /= 2;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
351 while ((m % 3) == 0) m /= 3;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
352 while ((m % 5) == 0) m /= 5;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
353 if (m <= 1)
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
354 break; /* n is completely factorable by twos, threes, and fives */
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
355 n++;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
356 }
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
357 return n;
|
Peter Meerwald <p.meerwald@bct-electronic.com>
parents:
diff
changeset
|
358 }
|