1 /* 2 * Copyright (C) 2020 Linux Studio Plugins Project <https://lsp-plug.in/> 3 * (C) 2020 Stefano Tronci <stefano.tronci@protonmail.com> 4 * 5 * This file is part of lsp-plugins 6 * Created on: 5 Apr 2017 7 * 8 * lsp-plugins is free software: you can redistribute it and/or modify 9 * it under the terms of the GNU Lesser General Public License as published by 10 * the Free Software Foundation, either version 3 of the License, or 11 * any later version. 12 * 13 * lsp-plugins is distributed in the hope that it will be useful, 14 * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 * GNU Lesser General Public License for more details. 17 * 18 * You should have received a copy of the GNU Lesser General Public License 19 * along with lsp-plugins. If not, see <https://www.gnu.org/licenses/>. 20 */ 21 22 #include <dsp/dsp.h> 23 #include <core/debug.h> 24 #include <core/util/LatencyDetector.h> 25 26 #define LIM_BUF_SIZE (1 << 15) 27 28 namespace lsp 29 { LatencyDetector()30 LatencyDetector::LatencyDetector() 31 { 32 nSampleRate = -1; 33 34 sChirpSystem.fDuration = 0.15; // 150 ms 35 sChirpSystem.fDelayRatio = 0.0f; 36 37 sChirpSystem.bModified = true; 38 39 sChirpSystem.nDuration = 0; 40 41 sChirpSystem.n2piMult = 0; 42 sChirpSystem.fAlpha = 0.0f; 43 sChirpSystem.fBeta = 0.0f; 44 sChirpSystem.nLength = 0; 45 sChirpSystem.nOrder = 0; 46 sChirpSystem.nFftRank = 0; 47 48 sChirpSystem.fConvScale = 0.0f; 49 50 sInputProcessor.nState = IP_BYPASS; 51 sInputProcessor.ig_time = 0; 52 sInputProcessor.ig_start = 0; 53 sInputProcessor.ig_stop = -1; 54 55 sInputProcessor.fDetect = 0.5f; // 500 ms 56 sInputProcessor.nDetect = 0; 57 sInputProcessor.nDetectCounter = 0; 58 59 sOutputProcessor.nState = OP_BYPASS; 60 sOutputProcessor.og_time = 0; 61 sOutputProcessor.og_start = 0; 62 63 sOutputProcessor.fGain = 1.0f; 64 sOutputProcessor.fGainDelta = 0.0f; 65 66 sOutputProcessor.fFade = 0.01; // 10 ms 67 sOutputProcessor.nFade = 0; 68 69 sOutputProcessor.fPause = 0.5f; // 500 ms 70 sOutputProcessor.nPause = 0; 71 sOutputProcessor.nPauseCounter = 0; 72 73 sOutputProcessor.nEmitCounter = 0; 74 75 sPeakDetector.fAbsThreshold = 0.0f; 76 sPeakDetector.fPeakThreshold = 0.0f; 77 sPeakDetector.fValue = 0.0f; 78 sPeakDetector.nPosition = 0; 79 sPeakDetector.nTimeOrigin = 0; 80 sPeakDetector.bDetected = false; 81 82 vChirp = NULL; 83 vAntiChirp = NULL; 84 vCapture = NULL; 85 vBuffer = NULL; 86 vChirpConv = NULL; 87 vConvBuf = NULL; 88 pData = NULL; 89 90 bCycleComplete = false; 91 bLatencyDetected = false; 92 nLatency = -1; 93 94 bSync = true; 95 } 96 ~LatencyDetector()97 LatencyDetector::~LatencyDetector() 98 { 99 } 100 init()101 void LatencyDetector::init() 102 { 103 // 1x chirp + 1x anti-chirp + 1x capture + 2x buffer + 4x conv image + 4x temporary convolution buffer 104 size_t samples = 13 * LIM_BUF_SIZE; 105 106 pData = new uint8_t[samples * sizeof(float) + DEFAULT_ALIGN]; 107 uint8_t *ptr = ALIGN_PTR(pData, DEFAULT_ALIGN); 108 109 vChirp = reinterpret_cast<float *>(ptr); 110 ptr += LIM_BUF_SIZE * sizeof(float); 111 vAntiChirp = reinterpret_cast<float *>(ptr); 112 ptr += LIM_BUF_SIZE * sizeof(float); 113 vCapture = reinterpret_cast<float *>(ptr); 114 ptr += LIM_BUF_SIZE * sizeof(float); 115 vBuffer = reinterpret_cast<float *>(ptr); 116 ptr += 2 * LIM_BUF_SIZE * sizeof(float); 117 vChirpConv = reinterpret_cast<float *>(ptr); 118 ptr += 4 * LIM_BUF_SIZE * sizeof(float); 119 vConvBuf = reinterpret_cast<float *>(ptr); 120 ptr += 4 * LIM_BUF_SIZE * sizeof(float); 121 122 dsp::fill_zero(vChirp, samples); 123 124 lsp_assert(ptr <= &pData[samples * sizeof(float) + DEFAULT_ALIGN]); 125 } 126 destroy()127 void LatencyDetector::destroy() 128 { 129 if (pData != NULL) 130 { 131 delete [] pData; 132 pData = NULL; 133 } 134 vChirp = NULL; 135 vAntiChirp = NULL; 136 vCapture = NULL; 137 vBuffer = NULL; 138 vChirpConv = NULL; 139 vConvBuf = NULL; 140 } 141 update_settings()142 void LatencyDetector::update_settings() 143 { 144 if (!bSync) 145 return; 146 147 // First of all, calculate parameters 148 149 if (sChirpSystem.bModified) 150 { 151 sChirpSystem.nDuration = seconds_to_samples(nSampleRate, sChirpSystem.fDuration); 152 153 /** Set up parameters. As all parameters are related to each other, this 154 * loop is needed to make sure the end duration in samples is not 155 * greater than LIM_BUF_SIZE 156 */ 157 while (true) 158 { 159 sChirpSystem.n2piMult = sChirpSystem.nDuration / (6.0f - sChirpSystem.fDelayRatio); 160 sChirpSystem.fAlpha = sChirpSystem.n2piMult * sChirpSystem.fDelayRatio; 161 162 if (sChirpSystem.nDuration <= (LIM_BUF_SIZE - sChirpSystem.fAlpha)) 163 break; 164 165 --sChirpSystem.nDuration; 166 } 167 168 sChirpSystem.fBeta = sChirpSystem.n2piMult * (2.0f - sChirpSystem.fDelayRatio) * M_1_PI; 169 170 /** Calculate the whole FIR number of samples as a power of 2. Thanks to 171 * the loop above it will be smaller or equal to LIM_BUF_SIZE 172 */ 173 sChirpSystem.nLength = 1; 174 sChirpSystem.nFftRank = 0; 175 while (sChirpSystem.nLength < (sChirpSystem.nDuration + sChirpSystem.fAlpha)) 176 { 177 sChirpSystem.nLength <<= 1; 178 ++sChirpSystem.nFftRank; 179 } 180 181 sChirpSystem.nOrder = sChirpSystem.nLength - 1; 182 183 /** For the frequency response, frequency sample up at which the 184 * frequency is positive 185 */ 186 size_t nPosFreqLim = (sChirpSystem.nLength >> 1) + 1; 187 188 /** Chirp system parameters are intended to be used in a frequency 189 * response as a function of standard DSP normalised frequency Omega: 190 * 191 * Omega = 2pi * frequency / SampleRate 192 * 193 * So the following parameter will convert from frequency sample 194 * number to normalised frequency value: 195 */ 196 float fSample2Omega = M_PI / nPosFreqLim; 197 198 /** The reference parabolic phase response is assumed positive in sign. 199 * So, the causal (direct) chirp frequency response has negative sign 200 * in the phase: 201 * 202 * H = exp(-i * Theta), Theta = alpha * Omega + beta * Omega^2 203 * 204 * This means that the group deleay is the simple derivative of Theta, 205 * no sign inverted: 206 * 207 * GD = dTheta / dOmega 208 */ 209 float *chirp_re = vChirpConv; 210 float *chirp_im = &vChirpConv[LIM_BUF_SIZE]; 211 212 for (size_t k = 0; k < nPosFreqLim; ++k) 213 { 214 float fOmega = k * fSample2Omega; 215 float angle = (sChirpSystem.fAlpha + sChirpSystem.fBeta * fOmega) * fOmega; 216 chirp_re[k] = cosf(angle); 217 chirp_im[k] = -sinf(angle); 218 } 219 220 /** Impose Hermitian symmetry (second half of the response is the first 221 * one, but flipped and conjugated). No repetitions of Omega = pi 222 * (Nyquist) and Omega = 2pi (Sample Rate). 223 */ 224 for (size_t k = nPosFreqLim; k < sChirpSystem.nLength; ++k) 225 { 226 size_t idx = ((nPosFreqLim - 1)<<1) - k; 227 chirp_re[k] = chirp_re[idx]; 228 chirp_im[k] = -chirp_im[idx]; 229 } 230 231 // Get the time domain samples through inverse fft: 232 // DSP FFT functions expect that all pointers are NON-NULL 233 // We don't require vHChirpIm so we can pass it as a target buffer 234 dsp::reverse_fft(vChirp, chirp_im, chirp_re, chirp_im, sChirpSystem.nFftRank); 235 236 // Normalise to avoid wasting dynamic range - Needs to introduce convolution scaling factors 237 float maxAbsChirp = dsp::abs_max(vChirp, sChirpSystem.nLength); 238 sChirpSystem.fConvScale = maxAbsChirp * maxAbsChirp; // This should make the convolution stay between 0 and 1 239 dsp::normalize(vChirp, vChirp, sChirpSystem.nLength); 240 241 // Create Anti Chirp samples by just reversing the chirp ones: 242 // Here we can use dsp::reverse2 243 dsp::reverse2(vAntiChirp, vChirp, sChirpSystem.nLength); 244 245 // Prepare the convolution image 246 dsp::fastconv_parse(vChirpConv, vAntiChirp, sChirpSystem.nFftRank+1); 247 248 sChirpSystem.bModified = false; 249 } 250 251 // Processors parameters: 252 sOutputProcessor.nFade = seconds_to_samples(nSampleRate, sOutputProcessor.fFade); 253 sOutputProcessor.fGainDelta = sOutputProcessor.fGain / (sOutputProcessor.nFade + 1); 254 sOutputProcessor.nPause = seconds_to_samples(nSampleRate, sOutputProcessor.fPause); 255 sInputProcessor.nDetect = sChirpSystem.nDuration + seconds_to_samples(nSampleRate, sInputProcessor.fDetect); 256 257 // Mark synced 258 bSync = false; 259 } 260 detect_peak(float * buf,size_t count)261 void LatencyDetector::detect_peak(float *buf, size_t count) 262 { 263 // Scan trough and update the highest absolute peak recorded. 264 // If the delta between the last highest peak recorded and the previous 265 // one is higher then threshold, produce early detection (provided that 266 // the latency makes sense). 267 268 size_t position = dsp::abs_max_index(buf, count); 269 float value = sChirpSystem.fConvScale * fabs(buf[position]); 270 271 if ((value > sPeakDetector.fAbsThreshold) && (value > sPeakDetector.fValue)) 272 { 273 float delta = value - sPeakDetector.fValue; 274 sPeakDetector.fValue = value; 275 sPeakDetector.nPosition = position + sInputProcessor.nDetectCounter - sChirpSystem.nLength; 276 277 nLatency = sPeakDetector.nPosition - sPeakDetector.nTimeOrigin; 278 279 // Early detection 280 if ((nLatency >= 0) && (delta > sPeakDetector.fPeakThreshold)) 281 { 282 bLatencyDetected = true; 283 sInputProcessor.nState = IP_BYPASS; 284 sOutputProcessor.nState = OP_FADEIN; 285 sInputProcessor.ig_stop = sInputProcessor.ig_time; 286 bCycleComplete = true; 287 } 288 } 289 } 290 process_in(float * dst,const float * src,size_t count)291 void LatencyDetector::process_in(float *dst, const float *src, size_t count) 292 { 293 if (bSync) 294 update_settings(); 295 296 while (count > 0) 297 { 298 switch (sInputProcessor.nState) 299 { 300 case IP_DETECT: 301 { 302 // Fill-in capture buffer 303 size_t captureIdx = sInputProcessor.nDetectCounter % sChirpSystem.nLength; 304 size_t to_do = sChirpSystem.nLength - captureIdx; 305 if (to_do > count) 306 to_do = count; 307 308 dsp::copy(&vCapture[captureIdx], src, to_do); 309 310 sInputProcessor.nDetectCounter += to_do; 311 sInputProcessor.ig_time += to_do; 312 dst += to_do; 313 src += to_do; 314 count -= to_do; 315 316 if ((sInputProcessor.nDetectCounter % sChirpSystem.nLength) == 0) 317 { 318 // Do the convolution 319 dsp::fastconv_parse_apply(vBuffer, vConvBuf, vChirpConv, vCapture, sChirpSystem.nFftRank+1); 320 321 detect_peak(vBuffer, sChirpSystem.nLength); 322 323 // Do post-actions after processing: shift convolution buffer to the chirp system length 324 dsp::move(vBuffer, &vBuffer[sChirpSystem.nLength], sChirpSystem.nLength); 325 } 326 327 // Force Processor transitions after the time allowed for detection have elapsed 328 if (sInputProcessor.nDetectCounter >= sInputProcessor.nDetect) 329 { 330 sInputProcessor.nState = IP_BYPASS; 331 sOutputProcessor.nState = OP_FADEIN; 332 sInputProcessor.ig_stop = sInputProcessor.ig_time; 333 bCycleComplete = true; 334 } 335 336 break; 337 } 338 339 case IP_WAIT: 340 sInputProcessor.ig_time += count; 341 dsp::copy(dst, src, count); 342 count = 0; 343 break; 344 case IP_BYPASS: 345 default: 346 dsp::copy(dst, src, count); 347 count = 0; 348 break; 349 } 350 } 351 } 352 process_out(float * dst,const float * src,size_t count)353 void LatencyDetector::process_out(float *dst, const float *src, size_t count) 354 { 355 if (bSync) 356 update_settings(); 357 358 while (count > 0) 359 { 360 switch (sOutputProcessor.nState) 361 { 362 case OP_FADEOUT: 363 while (count > 0) 364 { 365 sOutputProcessor.fGain -= sOutputProcessor.fGainDelta; 366 367 if (sOutputProcessor.fGain <= 0.0f) 368 { 369 sOutputProcessor.fGain = 0.0f; 370 sOutputProcessor.nPauseCounter = sOutputProcessor.nPause; 371 sOutputProcessor.nState = OP_PAUSE; 372 break; 373 } 374 375 *(dst++) = *(src++) * sOutputProcessor.fGain; 376 count --; 377 sOutputProcessor.og_time ++; 378 } 379 break; 380 381 case OP_PAUSE: 382 { 383 // For pause we're using decrementing counter 384 size_t to_do = (sOutputProcessor.nPauseCounter > count) ? count : sOutputProcessor.nPauseCounter; 385 dsp::fill_zero(dst, to_do); 386 387 sOutputProcessor.nPauseCounter -= to_do; 388 sOutputProcessor.og_time += to_do; 389 src += to_do; 390 dst += to_do; 391 count -= to_do; 392 393 if (sOutputProcessor.nPauseCounter <= 0) 394 { 395 sOutputProcessor.nEmitCounter = 0; 396 sOutputProcessor.nState = OP_EMIT; 397 sInputProcessor.nState = IP_DETECT; 398 sOutputProcessor.og_start = sOutputProcessor.og_time; 399 sInputProcessor.ig_start = sInputProcessor.ig_time; 400 sPeakDetector.fValue = 0.0f; 401 sPeakDetector.nPosition = 0; 402 // Correcting the apparent latency centre (nLength - 1) with the actually recorded samples: 403 sPeakDetector.nTimeOrigin = sChirpSystem.nLength - (sInputProcessor.ig_start - sOutputProcessor.og_start) - 1; 404 sPeakDetector.bDetected = false; 405 bLatencyDetected = false; 406 nLatency = 0; 407 408 dsp::fill_zero(vBuffer, 2 * LIM_BUF_SIZE); 409 } 410 break; 411 } 412 413 case OP_EMIT: 414 { 415 size_t to_do; 416 417 // For detection we're using incrementing counter 418 if (sOutputProcessor.nEmitCounter < sChirpSystem.nLength) 419 { 420 to_do = sChirpSystem.nLength - sOutputProcessor.nEmitCounter; 421 if (to_do > count) 422 to_do = count; 423 424 dsp::copy(dst, &vChirp[sOutputProcessor.nEmitCounter], to_do); 425 } 426 else 427 { 428 to_do = count; 429 dsp::fill_zero(dst, to_do); 430 } 431 432 sOutputProcessor.nEmitCounter += to_do; 433 sOutputProcessor.og_time += to_do; 434 dst += to_do; 435 src += to_do; 436 count -= to_do; 437 break; 438 } 439 440 case OP_FADEIN: 441 while (count > 0) 442 { 443 sOutputProcessor.fGain += sOutputProcessor.fGainDelta; 444 if (sOutputProcessor.fGain >= 1.0f) 445 { 446 sOutputProcessor.fGain = 1.0f; 447 sOutputProcessor.nState = OP_BYPASS; 448 break; 449 } 450 451 *(dst++) = *(src++) * sOutputProcessor.fGain; 452 count --; 453 sOutputProcessor.og_time ++; 454 } 455 break; 456 457 case OP_BYPASS: 458 default: 459 dsp::copy(dst, src, count); 460 count = 0; 461 break; 462 } 463 } 464 } 465 process(float * dst,const float * src,size_t count)466 void LatencyDetector::process(float *dst, const float *src, size_t count) 467 { 468 process_in(dst, src, count); 469 process_out(dst, dst, count); 470 } 471 } 472