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