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" { |
| 32 | #include "common/fft.h" |
| 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); |
| 83 | if (!subFilters) |
| 84 | return false; |
| 85 | |
| 86 | for (size_t i = 0; i < m; i++) { |
| 87 | subFilters[i] = (float *) |
| 88 | memalign(16, hLen * 2 * sizeof(float)); |
| 89 | } |
| 90 | |
| 91 | /* |
| 92 | * Generate the prototype filter with a Blackman-harris window. |
| 93 | * Scale coefficients with DC filter gain set to unity divided |
| 94 | * by the number of channels. |
| 95 | */ |
| 96 | float a0 = 0.35875; |
| 97 | float a1 = 0.48829; |
| 98 | float a2 = 0.14128; |
| 99 | float a3 = 0.01168; |
| 100 | |
| 101 | for (size_t i = 0; i < protoLen; i++) { |
| 102 | proto[i] = sinc(((float) i - midpt) / (float) m); |
| 103 | proto[i] *= a0 - |
| 104 | a1 * cos(2 * M_PI * i / (protoLen - 1)) + |
| 105 | a2 * cos(4 * M_PI * i / (protoLen - 1)) - |
| 106 | a3 * cos(6 * M_PI * i / (protoLen - 1)); |
| 107 | sum += proto[i]; |
| 108 | } |
| 109 | scale = (float) m / sum; |
| 110 | |
| 111 | /* |
| 112 | * Populate partition filters and reverse the coefficients per |
| 113 | * convolution requirements. |
| 114 | */ |
| 115 | for (size_t i = 0; i < hLen; i++) { |
| 116 | for (size_t n = 0; n < m; n++) { |
| 117 | subFilters[n][2 * i + 0] = proto[i * m + n] * scale; |
| 118 | subFilters[n][2 * i + 1] = 0.0f; |
| 119 | } |
| 120 | } |
| 121 | |
| 122 | for (size_t i = 0; i < m; i++) |
| 123 | reverse(subFilters[i], hLen); |
| 124 | |
| 125 | delete proto; |
| 126 | |
| 127 | return true; |
| 128 | } |
| 129 | |
| 130 | bool ChannelizerBase::initFFT() |
| 131 | { |
| 132 | size_t size; |
| 133 | |
| 134 | if (fftInput || fftOutput || fftHandle) |
| 135 | return false; |
| 136 | |
| 137 | size = blockLen * m * 2 * sizeof(float); |
| 138 | fftInput = (float *) fft_malloc(size); |
| 139 | memset(fftInput, 0, size); |
| 140 | |
| 141 | size = (blockLen + hLen) * m * 2 * sizeof(float); |
| 142 | fftOutput = (float *) fft_malloc(size); |
| 143 | memset(fftOutput, 0, size); |
| 144 | |
| 145 | if (!fftInput | !fftOutput) { |
| 146 | LOG(ALERT) << "Memory allocation error"; |
| 147 | return false; |
| 148 | } |
| 149 | |
| 150 | fftHandle = init_fft(0, m, blockLen, blockLen + hLen, |
| 151 | fftInput, fftOutput, hLen); |
| 152 | return true; |
| 153 | } |
| 154 | |
| 155 | bool ChannelizerBase::mapBuffers() |
| 156 | { |
| 157 | if (!fftHandle) { |
| 158 | LOG(ALERT) << "FFT buffers not initialized"; |
| 159 | return false; |
| 160 | } |
| 161 | |
| 162 | hInputs = (float **) malloc(sizeof(float *) * m); |
| 163 | hOutputs = (float **) malloc(sizeof(float *) * m); |
| 164 | if (!hInputs | !hOutputs) |
| 165 | return false; |
| 166 | |
| 167 | for (size_t i = 0; i < m; i++) { |
| 168 | hInputs[i] = &fftOutput[2 * (i * (blockLen + hLen) + hLen)]; |
| 169 | hOutputs[i] = &fftInput[2 * (i * blockLen)]; |
| 170 | } |
| 171 | |
| 172 | return true; |
| 173 | } |
| 174 | |
| 175 | /* |
| 176 | * Setup filterbank internals |
| 177 | */ |
| 178 | bool ChannelizerBase::init() |
| 179 | { |
| 180 | /* |
| 181 | * Filterbank coefficients, fft plan, history, and output sample |
| 182 | * rate conversion blocks |
| 183 | */ |
| 184 | if (!initFilters()) { |
| 185 | LOG(ALERT) << "Failed to initialize channelizing filter"; |
| 186 | return false; |
| 187 | } |
| 188 | |
| 189 | hist = (float **) malloc(sizeof(float *) * m); |
| 190 | for (size_t i = 0; i < m; i++) { |
| 191 | hist[i] = new float[2 * hLen]; |
| 192 | memset(hist[i], 0, 2 * hLen * sizeof(float)); |
| 193 | } |
| 194 | |
| 195 | if (!initFFT()) { |
| 196 | LOG(ALERT) << "Failed to initialize FFT"; |
| 197 | return false; |
| 198 | } |
| 199 | |
| 200 | mapBuffers(); |
| 201 | |
| 202 | return true; |
| 203 | } |
| 204 | |
| 205 | /* Check vector length validity */ |
| 206 | bool ChannelizerBase::checkLen(size_t innerLen, size_t outerLen) |
| 207 | { |
| 208 | if (outerLen != innerLen * m) { |
| 209 | LOG(ALERT) << "Invalid outer length " << innerLen |
| 210 | << " is not multiple of " << blockLen; |
| 211 | return false; |
| 212 | } |
| 213 | |
| 214 | if (innerLen != blockLen) { |
| 215 | LOG(ALERT) << "Invalid inner length " << outerLen |
| 216 | << " does not equal " << blockLen; |
| 217 | return false; |
| 218 | } |
| 219 | |
| 220 | return true; |
| 221 | } |
| 222 | |
| 223 | /* |
| 224 | * Setup channelizer paramaters |
| 225 | */ |
| 226 | ChannelizerBase::ChannelizerBase(size_t m, size_t blockLen, size_t hLen) |
| 227 | : fftInput(NULL), fftOutput(NULL), fftHandle(NULL) |
| 228 | { |
| 229 | this->m = m; |
| 230 | this->hLen = hLen; |
| 231 | this->blockLen = blockLen; |
| 232 | } |
| 233 | |
| 234 | ChannelizerBase::~ChannelizerBase() |
| 235 | { |
| 236 | free_fft(fftHandle); |
| 237 | |
| 238 | for (size_t i = 0; i < m; i++) { |
| 239 | free(subFilters[i]); |
| 240 | delete hist[i]; |
| 241 | } |
| 242 | |
| 243 | fft_free(fftInput); |
| 244 | fft_free(fftOutput); |
| 245 | |
| 246 | free(hInputs); |
| 247 | free(hOutputs); |
| 248 | free(hist); |
| 249 | } |