Tom Tsou | 3522229 | 2016-06-22 16:16:30 -0700 | [diff] [blame] | 1 | /* |
| 2 | * Polyphase channelizer |
| 3 | * |
| 4 | * Copyright (C) 2012-2014 Tom Tsou <tom@tsou.cc> |
| 5 | * Copyright (C) 2015 Ettus Research LLC |
| 6 | * |
| 7 | * This program is free software: you can redistribute it and/or modify |
| 8 | * it under the terms of the GNU Affero General Public License as published by |
| 9 | * the Free Software Foundation, either version 3 of the License, or |
| 10 | * (at your option) any later version. |
| 11 | * |
| 12 | * This program is distributed in the hope that it will be useful, |
| 13 | * but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 14 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| 15 | * GNU Affero General Public License for more details. |
| 16 | * |
| 17 | * You should have received a copy of the GNU Affero General Public License |
| 18 | * along with this program. If not, see <http://www.gnu.org/licenses/>. |
| 19 | * See the COPYING file in the main directory for details. |
| 20 | */ |
| 21 | |
| 22 | #include <malloc.h> |
| 23 | #include <math.h> |
| 24 | #include <assert.h> |
| 25 | #include <string.h> |
| 26 | #include <cstdio> |
| 27 | |
| 28 | #include "Logger.h" |
| 29 | #include "ChannelizerBase.h" |
| 30 | |
| 31 | extern "C" { |
Pau Espin Pedrol | 43fedb6 | 2018-04-24 15:22:57 +0200 | [diff] [blame] | 32 | #include "fft.h" |
Tom Tsou | 3522229 | 2016-06-22 16:16:30 -0700 | [diff] [blame] | 33 | } |
| 34 | |
| 35 | static float sinc(float x) |
| 36 | { |
| 37 | if (x == 0.0f) |
| 38 | return 0.999999999999f; |
| 39 | |
| 40 | return sin(M_PI * x) / (M_PI * x); |
| 41 | } |
| 42 | |
| 43 | /* |
| 44 | * There are more efficient reversal algorithms, but we only reverse at |
| 45 | * initialization so we don't care. |
| 46 | */ |
| 47 | static void reverse(float *buf, size_t len) |
| 48 | { |
| 49 | float tmp[2 * len]; |
| 50 | memcpy(tmp, buf, 2 * len * sizeof(float)); |
| 51 | |
| 52 | for (size_t i = 0; i < len; i++) { |
| 53 | buf[2 * i + 0] = tmp[2 * (len - 1 - i) + 0]; |
| 54 | buf[2 * i + 1] = tmp[2 * (len - 1 - i) + 1]; |
| 55 | } |
| 56 | } |
| 57 | |
| 58 | /* |
| 59 | * Create polyphase filterbank |
| 60 | * |
| 61 | * Implementation based material found in, |
| 62 | * |
| 63 | * "harris, fred, Multirate Signal Processing, Upper Saddle River, NJ, |
| 64 | * Prentice Hall, 2006." |
| 65 | */ |
| 66 | bool ChannelizerBase::initFilters() |
| 67 | { |
| 68 | size_t protoLen = m * hLen; |
| 69 | float *proto; |
| 70 | float sum = 0.0f, scale = 0.0f; |
| 71 | float midpt = (float) (protoLen - 1.0) / 2.0; |
| 72 | |
| 73 | /* |
| 74 | * Allocate 'M' partition filters and the temporary prototype |
| 75 | * filter. Coefficients are real only and must be 16-byte memory |
| 76 | * aligned for SSE usage. |
| 77 | */ |
| 78 | proto = new float[protoLen]; |
| 79 | if (!proto) |
| 80 | return false; |
| 81 | |
| 82 | subFilters = (float **) malloc(sizeof(float *) * m); |
pierre.baudry | 9436fbb | 2016-10-20 16:30:51 +0200 | [diff] [blame] | 83 | if (!subFilters) { |
| 84 | delete[] proto; |
Tom Tsou | 3522229 | 2016-06-22 16:16:30 -0700 | [diff] [blame] | 85 | return false; |
pierre.baudry | 9436fbb | 2016-10-20 16:30:51 +0200 | [diff] [blame] | 86 | } |
Tom Tsou | 3522229 | 2016-06-22 16:16:30 -0700 | [diff] [blame] | 87 | |
| 88 | for (size_t i = 0; i < m; i++) { |
| 89 | subFilters[i] = (float *) |
| 90 | memalign(16, hLen * 2 * sizeof(float)); |
| 91 | } |
| 92 | |
| 93 | /* |
| 94 | * Generate the prototype filter with a Blackman-harris window. |
| 95 | * Scale coefficients with DC filter gain set to unity divided |
| 96 | * by the number of channels. |
| 97 | */ |
| 98 | float a0 = 0.35875; |
| 99 | float a1 = 0.48829; |
| 100 | float a2 = 0.14128; |
| 101 | float a3 = 0.01168; |
| 102 | |
| 103 | for (size_t i = 0; i < protoLen; i++) { |
| 104 | proto[i] = sinc(((float) i - midpt) / (float) m); |
| 105 | proto[i] *= a0 - |
| 106 | a1 * cos(2 * M_PI * i / (protoLen - 1)) + |
| 107 | a2 * cos(4 * M_PI * i / (protoLen - 1)) - |
| 108 | a3 * cos(6 * M_PI * i / (protoLen - 1)); |
| 109 | sum += proto[i]; |
| 110 | } |
| 111 | scale = (float) m / sum; |
| 112 | |
| 113 | /* |
| 114 | * Populate partition filters and reverse the coefficients per |
| 115 | * convolution requirements. |
| 116 | */ |
| 117 | for (size_t i = 0; i < hLen; i++) { |
| 118 | for (size_t n = 0; n < m; n++) { |
| 119 | subFilters[n][2 * i + 0] = proto[i * m + n] * scale; |
| 120 | subFilters[n][2 * i + 1] = 0.0f; |
| 121 | } |
| 122 | } |
| 123 | |
| 124 | for (size_t i = 0; i < m; i++) |
| 125 | reverse(subFilters[i], hLen); |
| 126 | |
pierre.baudry | 9436fbb | 2016-10-20 16:30:51 +0200 | [diff] [blame] | 127 | delete[] proto; |
Tom Tsou | 3522229 | 2016-06-22 16:16:30 -0700 | [diff] [blame] | 128 | |
| 129 | return true; |
| 130 | } |
| 131 | |
| 132 | bool ChannelizerBase::initFFT() |
| 133 | { |
| 134 | size_t size; |
| 135 | |
| 136 | if (fftInput || fftOutput || fftHandle) |
| 137 | return false; |
| 138 | |
| 139 | size = blockLen * m * 2 * sizeof(float); |
| 140 | fftInput = (float *) fft_malloc(size); |
| 141 | memset(fftInput, 0, size); |
| 142 | |
| 143 | size = (blockLen + hLen) * m * 2 * sizeof(float); |
| 144 | fftOutput = (float *) fft_malloc(size); |
| 145 | memset(fftOutput, 0, size); |
| 146 | |
| 147 | if (!fftInput | !fftOutput) { |
| 148 | LOG(ALERT) << "Memory allocation error"; |
| 149 | return false; |
| 150 | } |
| 151 | |
| 152 | fftHandle = init_fft(0, m, blockLen, blockLen + hLen, |
| 153 | fftInput, fftOutput, hLen); |
| 154 | return true; |
| 155 | } |
| 156 | |
| 157 | bool ChannelizerBase::mapBuffers() |
| 158 | { |
| 159 | if (!fftHandle) { |
| 160 | LOG(ALERT) << "FFT buffers not initialized"; |
| 161 | return false; |
| 162 | } |
| 163 | |
| 164 | hInputs = (float **) malloc(sizeof(float *) * m); |
| 165 | hOutputs = (float **) malloc(sizeof(float *) * m); |
| 166 | if (!hInputs | !hOutputs) |
| 167 | return false; |
| 168 | |
| 169 | for (size_t i = 0; i < m; i++) { |
| 170 | hInputs[i] = &fftOutput[2 * (i * (blockLen + hLen) + hLen)]; |
| 171 | hOutputs[i] = &fftInput[2 * (i * blockLen)]; |
| 172 | } |
| 173 | |
| 174 | return true; |
| 175 | } |
| 176 | |
| 177 | /* |
| 178 | * Setup filterbank internals |
| 179 | */ |
| 180 | bool ChannelizerBase::init() |
| 181 | { |
| 182 | /* |
| 183 | * Filterbank coefficients, fft plan, history, and output sample |
| 184 | * rate conversion blocks |
| 185 | */ |
| 186 | if (!initFilters()) { |
| 187 | LOG(ALERT) << "Failed to initialize channelizing filter"; |
| 188 | return false; |
| 189 | } |
| 190 | |
| 191 | hist = (float **) malloc(sizeof(float *) * m); |
| 192 | for (size_t i = 0; i < m; i++) { |
| 193 | hist[i] = new float[2 * hLen]; |
| 194 | memset(hist[i], 0, 2 * hLen * sizeof(float)); |
| 195 | } |
| 196 | |
| 197 | if (!initFFT()) { |
| 198 | LOG(ALERT) << "Failed to initialize FFT"; |
| 199 | return false; |
| 200 | } |
| 201 | |
| 202 | mapBuffers(); |
| 203 | |
| 204 | return true; |
| 205 | } |
| 206 | |
| 207 | /* Check vector length validity */ |
| 208 | bool ChannelizerBase::checkLen(size_t innerLen, size_t outerLen) |
| 209 | { |
| 210 | if (outerLen != innerLen * m) { |
| 211 | LOG(ALERT) << "Invalid outer length " << innerLen |
| 212 | << " is not multiple of " << blockLen; |
| 213 | return false; |
| 214 | } |
| 215 | |
| 216 | if (innerLen != blockLen) { |
| 217 | LOG(ALERT) << "Invalid inner length " << outerLen |
| 218 | << " does not equal " << blockLen; |
| 219 | return false; |
| 220 | } |
| 221 | |
| 222 | return true; |
| 223 | } |
| 224 | |
| 225 | /* |
| 226 | * Setup channelizer paramaters |
| 227 | */ |
| 228 | ChannelizerBase::ChannelizerBase(size_t m, size_t blockLen, size_t hLen) |
| 229 | : fftInput(NULL), fftOutput(NULL), fftHandle(NULL) |
| 230 | { |
| 231 | this->m = m; |
| 232 | this->hLen = hLen; |
| 233 | this->blockLen = blockLen; |
| 234 | } |
| 235 | |
| 236 | ChannelizerBase::~ChannelizerBase() |
| 237 | { |
| 238 | free_fft(fftHandle); |
| 239 | |
| 240 | for (size_t i = 0; i < m; i++) { |
| 241 | free(subFilters[i]); |
Pau Espin Pedrol | 08dfe23 | 2018-11-01 18:23:29 +0100 | [diff] [blame] | 242 | delete[] hist[i]; |
Tom Tsou | 3522229 | 2016-06-22 16:16:30 -0700 | [diff] [blame] | 243 | } |
| 244 | |
| 245 | fft_free(fftInput); |
| 246 | fft_free(fftOutput); |
| 247 | |
| 248 | free(hInputs); |
| 249 | free(hOutputs); |
| 250 | free(hist); |
| 251 | } |