Commit | Line | Data |
---|---|---|
3719602c PC |
1 | /* Copyright (C) 2010-2020 The RetroArch team |
2 | * | |
3 | * --------------------------------------------------------------------------------------- | |
4 | * The following license statement only applies to this file (fft.c). | |
5 | * --------------------------------------------------------------------------------------- | |
6 | * | |
7 | * Permission is hereby granted, free of charge, | |
8 | * to any person obtaining a copy of this software and associated documentation files (the "Software"), | |
9 | * to deal in the Software without restriction, including without limitation the rights to | |
10 | * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, | |
11 | * and to permit persons to whom the Software is furnished to do so, subject to the following conditions: | |
12 | * | |
13 | * The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. | |
14 | * | |
15 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, | |
16 | * INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | |
17 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. | |
18 | * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, | |
19 | * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | |
20 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. | |
21 | */ | |
22 | ||
23 | #include <math.h> | |
24 | #include <stdlib.h> | |
25 | ||
26 | #include "fft.h" | |
27 | ||
28 | #include <retro_miscellaneous.h> | |
29 | ||
30 | struct fft | |
31 | { | |
32 | fft_complex_t *interleave_buffer; | |
33 | fft_complex_t *phase_lut; | |
34 | unsigned *bitinverse_buffer; | |
35 | unsigned size; | |
36 | }; | |
37 | ||
38 | static unsigned bitswap(unsigned x, unsigned size_log2) | |
39 | { | |
40 | unsigned i; | |
41 | unsigned ret = 0; | |
42 | for (i = 0; i < size_log2; i++) | |
43 | ret |= ((x >> i) & 1) << (size_log2 - i - 1); | |
44 | return ret; | |
45 | } | |
46 | ||
47 | static void build_bitinverse(unsigned *bitinverse, unsigned size_log2) | |
48 | { | |
49 | unsigned i; | |
50 | unsigned size = 1 << size_log2; | |
51 | for (i = 0; i < size; i++) | |
52 | bitinverse[i] = bitswap(i, size_log2); | |
53 | } | |
54 | ||
55 | static fft_complex_t exp_imag(double phase) | |
56 | { | |
57 | fft_complex_t out = { cos(phase), sin(phase) }; | |
58 | return out; | |
59 | } | |
60 | ||
61 | static void build_phase_lut(fft_complex_t *out, int size) | |
62 | { | |
63 | int i; | |
64 | out += size; | |
65 | for (i = -size; i <= size; i++) | |
66 | out[i] = exp_imag((M_PI * i) / size); | |
67 | } | |
68 | ||
69 | static void interleave_complex(const unsigned *bitinverse, | |
70 | fft_complex_t *out, const fft_complex_t *in, | |
71 | unsigned samples, unsigned step) | |
72 | { | |
73 | unsigned i; | |
74 | for (i = 0; i < samples; i++, in += step) | |
75 | out[bitinverse[i]] = *in; | |
76 | } | |
77 | ||
78 | static void interleave_float(const unsigned *bitinverse, | |
79 | fft_complex_t *out, const float *in, | |
80 | unsigned samples, unsigned step) | |
81 | { | |
82 | unsigned i; | |
83 | for (i = 0; i < samples; i++, in += step) | |
84 | { | |
85 | unsigned inv_i = bitinverse[i]; | |
86 | out[inv_i].real = *in; | |
87 | out[inv_i].imag = 0.0f; | |
88 | } | |
89 | } | |
90 | ||
91 | static void resolve_float(float *out, const fft_complex_t *in, unsigned samples, | |
92 | float gain, unsigned step) | |
93 | { | |
94 | unsigned i; | |
95 | for (i = 0; i < samples; i++, in++, out += step) | |
96 | *out = gain * in->real; | |
97 | } | |
98 | ||
99 | fft_t *fft_new(unsigned block_size_log2) | |
100 | { | |
101 | unsigned size; | |
102 | fft_t *fft = (fft_t*)calloc(1, sizeof(*fft)); | |
103 | if (!fft) | |
104 | return NULL; | |
105 | ||
106 | size = 1 << block_size_log2; | |
107 | fft->interleave_buffer = (fft_complex_t*)calloc(size, sizeof(*fft->interleave_buffer)); | |
108 | fft->bitinverse_buffer = (unsigned*)calloc(size, sizeof(*fft->bitinverse_buffer)); | |
109 | fft->phase_lut = (fft_complex_t*)calloc(2 * size + 1, sizeof(*fft->phase_lut)); | |
110 | ||
111 | if (!fft->interleave_buffer || !fft->bitinverse_buffer || !fft->phase_lut) | |
112 | goto error; | |
113 | ||
114 | fft->size = size; | |
115 | ||
116 | build_bitinverse(fft->bitinverse_buffer, block_size_log2); | |
117 | build_phase_lut(fft->phase_lut, size); | |
118 | return fft; | |
119 | ||
120 | error: | |
121 | fft_free(fft); | |
122 | return NULL; | |
123 | } | |
124 | ||
125 | void fft_free(fft_t *fft) | |
126 | { | |
127 | if (!fft) | |
128 | return; | |
129 | ||
130 | free(fft->interleave_buffer); | |
131 | free(fft->bitinverse_buffer); | |
132 | free(fft->phase_lut); | |
133 | free(fft); | |
134 | } | |
135 | ||
136 | static void butterfly(fft_complex_t *a, fft_complex_t *b, fft_complex_t mod) | |
137 | { | |
138 | mod = fft_complex_mul(mod, *b); | |
139 | *b = fft_complex_sub(*a, mod); | |
140 | *a = fft_complex_add(*a, mod); | |
141 | } | |
142 | ||
143 | static void butterflies(fft_complex_t *butterfly_buf, | |
144 | const fft_complex_t *phase_lut, | |
145 | int phase_dir, unsigned step_size, unsigned samples) | |
146 | { | |
147 | unsigned i, j; | |
148 | for (i = 0; i < samples; i += step_size << 1) | |
149 | { | |
150 | int phase_step = (int)samples * phase_dir / (int)step_size; | |
151 | for (j = i; j < i + step_size; j++) | |
152 | butterfly(&butterfly_buf[j], &butterfly_buf[j + step_size], | |
153 | phase_lut[phase_step * (int)(j - i)]); | |
154 | } | |
155 | } | |
156 | ||
157 | void fft_process_forward_complex(fft_t *fft, | |
158 | fft_complex_t *out, const fft_complex_t *in, unsigned step) | |
159 | { | |
160 | unsigned step_size; | |
161 | unsigned samples = fft->size; | |
162 | interleave_complex(fft->bitinverse_buffer, out, in, samples, step); | |
163 | ||
164 | for (step_size = 1; step_size < samples; step_size <<= 1) | |
165 | { | |
166 | butterflies(out, | |
167 | fft->phase_lut + samples, | |
168 | -1, step_size, samples); | |
169 | } | |
170 | } | |
171 | ||
172 | void fft_process_forward(fft_t *fft, | |
173 | fft_complex_t *out, const float *in, unsigned step) | |
174 | { | |
175 | unsigned step_size; | |
176 | unsigned samples = fft->size; | |
177 | interleave_float(fft->bitinverse_buffer, out, in, samples, step); | |
178 | ||
179 | for (step_size = 1; step_size < fft->size; step_size <<= 1) | |
180 | { | |
181 | butterflies(out, | |
182 | fft->phase_lut + samples, | |
183 | -1, step_size, samples); | |
184 | } | |
185 | } | |
186 | ||
187 | void fft_process_inverse(fft_t *fft, | |
188 | float *out, const fft_complex_t *in, unsigned step) | |
189 | { | |
190 | unsigned step_size; | |
191 | unsigned samples = fft->size; | |
192 | ||
193 | interleave_complex(fft->bitinverse_buffer, fft->interleave_buffer, | |
194 | in, samples, 1); | |
195 | ||
196 | for (step_size = 1; step_size < samples; step_size <<= 1) | |
197 | { | |
198 | butterflies(fft->interleave_buffer, | |
199 | fft->phase_lut + samples, | |
200 | 1, step_size, samples); | |
201 | } | |
202 | ||
203 | resolve_float(out, fft->interleave_buffer, samples, 1.0f / samples, step); | |
204 | } |