1a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten/* 2a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten * Copyright (C) 2010 The Android Open Source Project 3a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten * 4a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten * Licensed under the Apache License, Version 2.0 (the "License"); 5a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten * you may not use this file except in compliance with the License. 6a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten * You may obtain a copy of the License at 7a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten * 8a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten * http://www.apache.org/licenses/LICENSE-2.0 9a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten * 10a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten * Unless required by applicable law or agreed to in writing, software 11a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten * distributed under the License is distributed on an "AS IS" BASIS, 12a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten * See the License for the specific language governing permissions and 14a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten * limitations under the License. 15a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten */ 16a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 17a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten/* A Fixed point implementation of Fast Fourier Transform (FFT). Complex numbers 18a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten * are represented by 32-bit integers, where higher 16 bits are real part and 19a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten * lower ones are imaginary part. Few compromises are made between efficiency, 20a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten * accuracy, and maintainability. To make it fast, arithmetic shifts are used 21a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten * instead of divisions, and bitwise inverses are used instead of negates. To 22a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten * keep it small, only radix-2 Cooley-Tukey algorithm is implemented, and only 23a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten * half of the twiddle factors are stored. Although there are still ways to make 24a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten * it even faster or smaller, it costs too much on one of the aspects. 25a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten */ 26a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 27a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten#include <stdio.h> 28a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten#include <stdint.h> 29a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 30a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten#include <audio_utils/fixedfft.h> 31a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 32a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten#define LOG_FFT_SIZE 10 33a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten#define MAX_FFT_SIZE (1 << LOG_FFT_SIZE) 34a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 35237381517db4b09e3b32a683f5561747ca1f8835Glenn Kasten// Actually int32_t, but declare as uint32_t to avoid warnings due to overflow. 36237381517db4b09e3b32a683f5561747ca1f8835Glenn Kasten// Be sure to cast all accesses before use, for example "(int32_t) twiddle[...]". 37237381517db4b09e3b32a683f5561747ca1f8835Glenn Kastenstatic const uint32_t twiddle[MAX_FFT_SIZE / 4] = { 38a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0x00008000, 0xff378001, 0xfe6e8002, 0xfda58006, 0xfcdc800a, 0xfc13800f, 39a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0xfb4a8016, 0xfa81801e, 0xf9b88027, 0xf8ef8032, 0xf827803e, 0xf75e804b, 40a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0xf6958059, 0xf5cd8068, 0xf5058079, 0xf43c808b, 0xf374809e, 0xf2ac80b2, 41a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0xf1e480c8, 0xf11c80de, 0xf05580f6, 0xef8d8110, 0xeec6812a, 0xedff8146, 42a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0xed388163, 0xec718181, 0xebab81a0, 0xeae481c1, 0xea1e81e2, 0xe9588205, 43a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0xe892822a, 0xe7cd824f, 0xe7078276, 0xe642829d, 0xe57d82c6, 0xe4b982f1, 44a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0xe3f4831c, 0xe3308349, 0xe26d8377, 0xe1a983a6, 0xe0e683d6, 0xe0238407, 45a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0xdf61843a, 0xde9e846e, 0xdddc84a3, 0xdd1b84d9, 0xdc598511, 0xdb998549, 46a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0xdad88583, 0xda1885be, 0xd95885fa, 0xd8988637, 0xd7d98676, 0xd71b86b6, 47a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0xd65c86f6, 0xd59e8738, 0xd4e1877b, 0xd42487c0, 0xd3678805, 0xd2ab884c, 48a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0xd1ef8894, 0xd13488dd, 0xd0798927, 0xcfbe8972, 0xcf0489be, 0xce4b8a0c, 49a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0xcd928a5a, 0xccd98aaa, 0xcc218afb, 0xcb698b4d, 0xcab28ba0, 0xc9fc8bf5, 50a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0xc9468c4a, 0xc8908ca1, 0xc7db8cf8, 0xc7278d51, 0xc6738dab, 0xc5c08e06, 51a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0xc50d8e62, 0xc45b8ebf, 0xc3a98f1d, 0xc2f88f7d, 0xc2488fdd, 0xc198903e, 52a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0xc0e990a1, 0xc03a9105, 0xbf8c9169, 0xbedf91cf, 0xbe329236, 0xbd86929e, 53a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0xbcda9307, 0xbc2f9371, 0xbb8593dc, 0xbadc9448, 0xba3394b5, 0xb98b9523, 54a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0xb8e39592, 0xb83c9603, 0xb7969674, 0xb6f196e6, 0xb64c9759, 0xb5a897ce, 55a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0xb5059843, 0xb46298b9, 0xb3c09930, 0xb31f99a9, 0xb27f9a22, 0xb1df9a9c, 56a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0xb1409b17, 0xb0a29b94, 0xb0059c11, 0xaf689c8f, 0xaecc9d0e, 0xae319d8e, 57a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0xad979e0f, 0xacfd9e91, 0xac659f14, 0xabcd9f98, 0xab36a01c, 0xaaa0a0a2, 58a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0xaa0aa129, 0xa976a1b0, 0xa8e2a238, 0xa84fa2c2, 0xa7bda34c, 0xa72ca3d7, 59a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0xa69ca463, 0xa60ca4f0, 0xa57ea57e, 0xa4f0a60c, 0xa463a69c, 0xa3d7a72c, 60a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0xa34ca7bd, 0xa2c2a84f, 0xa238a8e2, 0xa1b0a976, 0xa129aa0a, 0xa0a2aaa0, 61a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0xa01cab36, 0x9f98abcd, 0x9f14ac65, 0x9e91acfd, 0x9e0fad97, 0x9d8eae31, 62a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0x9d0eaecc, 0x9c8faf68, 0x9c11b005, 0x9b94b0a2, 0x9b17b140, 0x9a9cb1df, 63a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0x9a22b27f, 0x99a9b31f, 0x9930b3c0, 0x98b9b462, 0x9843b505, 0x97ceb5a8, 64a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0x9759b64c, 0x96e6b6f1, 0x9674b796, 0x9603b83c, 0x9592b8e3, 0x9523b98b, 65a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0x94b5ba33, 0x9448badc, 0x93dcbb85, 0x9371bc2f, 0x9307bcda, 0x929ebd86, 66a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0x9236be32, 0x91cfbedf, 0x9169bf8c, 0x9105c03a, 0x90a1c0e9, 0x903ec198, 67a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0x8fddc248, 0x8f7dc2f8, 0x8f1dc3a9, 0x8ebfc45b, 0x8e62c50d, 0x8e06c5c0, 68a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0x8dabc673, 0x8d51c727, 0x8cf8c7db, 0x8ca1c890, 0x8c4ac946, 0x8bf5c9fc, 69a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0x8ba0cab2, 0x8b4dcb69, 0x8afbcc21, 0x8aaaccd9, 0x8a5acd92, 0x8a0cce4b, 70a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0x89becf04, 0x8972cfbe, 0x8927d079, 0x88ddd134, 0x8894d1ef, 0x884cd2ab, 71a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0x8805d367, 0x87c0d424, 0x877bd4e1, 0x8738d59e, 0x86f6d65c, 0x86b6d71b, 72a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0x8676d7d9, 0x8637d898, 0x85fad958, 0x85beda18, 0x8583dad8, 0x8549db99, 73a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0x8511dc59, 0x84d9dd1b, 0x84a3dddc, 0x846ede9e, 0x843adf61, 0x8407e023, 74a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0x83d6e0e6, 0x83a6e1a9, 0x8377e26d, 0x8349e330, 0x831ce3f4, 0x82f1e4b9, 75a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0x82c6e57d, 0x829de642, 0x8276e707, 0x824fe7cd, 0x822ae892, 0x8205e958, 76a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0x81e2ea1e, 0x81c1eae4, 0x81a0ebab, 0x8181ec71, 0x8163ed38, 0x8146edff, 77a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0x812aeec6, 0x8110ef8d, 0x80f6f055, 0x80def11c, 0x80c8f1e4, 0x80b2f2ac, 78a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0x809ef374, 0x808bf43c, 0x8079f505, 0x8068f5cd, 0x8059f695, 0x804bf75e, 79a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0x803ef827, 0x8032f8ef, 0x8027f9b8, 0x801efa81, 0x8016fb4a, 0x800ffc13, 80a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 0x800afcdc, 0x8006fda5, 0x8002fe6e, 0x8001ff37, 81a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten}; 82a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 83a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten/* Returns the multiplication of \conj{a} and {b}. */ 84a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kastenstatic inline int32_t mult(int32_t a, int32_t b) 85a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten{ 8699dc78ca46bceb9cc4b5dc68a0990bc1b9b3882fElliott Hughes#if defined(__arm__) 87a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten int32_t t = b; 88a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten __asm__("smuad %0, %0, %1" : "+r" (t) : "r" (a)); 89a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten __asm__("smusdx %0, %0, %1" : "+r" (b) : "r" (a)); 90a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten __asm__("pkhtb %0, %0, %1, ASR #16" : "+r" (t) : "r" (b)); 91a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten return t; 92a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten#else 93a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten return (((a >> 16) * (b >> 16) + (int16_t)a * (int16_t)b) & ~0xFFFF) | 94a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten ((((a >> 16) * (int16_t)b - (int16_t)a * (b >> 16)) >> 16) & 0xFFFF); 95a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten#endif 96a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten} 97a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 98a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kastenstatic inline int32_t half(int32_t a) 99a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten{ 10099dc78ca46bceb9cc4b5dc68a0990bc1b9b3882fElliott Hughes#if defined(__arm__) 101a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten __asm__("shadd16 %0, %0, %1" : "+r" (a) : "r" (0)); 102a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten return a; 103a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten#else 104a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten return ((a >> 1) & ~0x8000) | (a & 0x8000); 105a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten#endif 106a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten} 107a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 108a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kastenvoid fixed_fft(int n, int32_t *v) 109a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten{ 110a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten int scale = LOG_FFT_SIZE, i, p, r; 111a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 112a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten for (r = 0, i = 1; i < n; ++i) { 113a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten for (p = n; !(p & r); p >>= 1, r ^= p); 114a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten if (i < r) { 115a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten int32_t t = v[i]; 116a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten v[i] = v[r]; 117a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten v[r] = t; 118a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten } 119a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten } 120a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 121a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten for (p = 1; p < n; p <<= 1) { 122a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten --scale; 123a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 124a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten for (i = 0; i < n; i += p << 1) { 125a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten int32_t x = half(v[i]); 126a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten int32_t y = half(v[i + p]); 127a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten v[i] = x + y; 128a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten v[i + p] = x - y; 129a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten } 130a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 131a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten for (r = 1; r < p; ++r) { 132a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten int32_t w = MAX_FFT_SIZE / 4 - (r << scale); 133a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten i = w >> 31; 134237381517db4b09e3b32a683f5561747ca1f8835Glenn Kasten w = ((int32_t) twiddle[(w ^ i) - i]) ^ (i << 16); 135a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten for (i = r; i < n; i += p << 1) { 136a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten int32_t x = half(v[i]); 137a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten int32_t y = mult(w, v[i + p]); 138a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten v[i] = x - y; 139a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten v[i + p] = x + y; 140a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten } 141a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten } 142a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten } 143a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten} 144a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 145a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kastenvoid fixed_fft_real(int n, int32_t *v) 146a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten{ 147a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten int scale = LOG_FFT_SIZE, m = n >> 1, i; 148a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 149a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten fixed_fft(n, v); 150a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten for (i = 1; i <= n; i <<= 1, --scale); 151a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten v[0] = mult(~v[0], 0x80008000); 152a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten v[m] = half(v[m]); 153a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten 154a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten for (i = 1; i < n >> 1; ++i) { 155a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten int32_t x = half(v[i]); 156a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten int32_t z = half(v[n - i]); 157a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten int32_t y = z - (x ^ 0xFFFF); 158a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten x = half(x + (z ^ 0xFFFF)); 159237381517db4b09e3b32a683f5561747ca1f8835Glenn Kasten y = mult(y, ((int32_t) twiddle[i << scale])); 160a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten v[i] = x - y; 161a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten v[n - i] = (x + y) ^ 0xFFFF; 162a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten } 163a269f35b6247cb69e8815b84440bf1bfc938b87bGlenn Kasten} 164