pw_decoder.cpp (12962B)
1 #include "pw_decoder.hpp" 2 3 #include <algorithm> 4 #include <cmath> 5 #include <cstring> 6 7 // Based on PipeWire 8 /* SPDX-FileCopyrightText: Copyright © 2018 Wim Taymans */ 9 /* SPDX-License-Identifier: MIT */ 10 11 template <std::size_t NConseq = 1> 12 static void Vol(float* dst, std::size_t dst_stride, 13 const float* src, std::size_t src_stride, std::size_t n_samples, 14 float vol) 15 { 16 if (vol == 0.f) 17 while (n_samples--) 18 { 19 for (std::size_t i = 0; i < NConseq; ++i) 20 dst[i] = 0.f; 21 dst += dst_stride; 22 } 23 else if (vol == 1.f) 24 while (n_samples--) 25 { 26 for (std::size_t i = 0; i < NConseq; ++i) 27 dst[i] = src[i]; 28 dst += dst_stride; 29 src += src_stride; 30 } 31 else 32 while (n_samples--) 33 { 34 for (std::size_t i = 0; i < NConseq; ++i) 35 dst[i] = src[i] * vol; 36 dst += dst_stride; 37 src += src_stride; 38 } 39 } 40 41 static void Avg(float* dst, std::size_t dst_stride, std::span<const float> src) 42 { 43 auto s = src.data(); 44 for (auto n = src.size(); n; n -= 2) 45 { 46 *dst = (s[0] + s[1]) * .5f; 47 dst += dst_stride; 48 s += 2; 49 } 50 } 51 52 static void Sub(float* dst, std::size_t dst_stride, std::span<const float> src) 53 { 54 auto s = src.data(); 55 for (auto n = src.size(); n; n -= 2) 56 { 57 *dst = s[0] - s[1]; 58 dst += dst_stride; 59 s += 2; 60 } 61 } 62 63 64 static void lr4_process_c( 65 struct lr4 *lr4, float *dst, std::size_t dst_stride, 66 const float *src, std::size_t src_stride, const float vol, std::size_t samples) 67 { 68 float x1 = lr4->x1; 69 float x2 = lr4->x2; 70 float y1 = lr4->y1; 71 float y2 = lr4->y2; 72 float b0 = lr4->bq.b0; 73 float b1 = lr4->bq.b1; 74 float b2 = lr4->bq.b2; 75 float a1 = lr4->bq.a1; 76 float a2 = lr4->bq.a2; 77 float x, y, z; 78 79 if (vol == 0.0f || !lr4->active) { 80 Vol(dst, dst_stride, src, src_stride, samples, vol); 81 return; 82 } 83 84 for (std::size_t i = 0; i < samples; i++) { 85 x = *src; 86 y = b0 * x + x1; 87 x1 = b1 * x - a1 * y + x2; 88 x2 = b2 * x - a2 * y; 89 z = b0 * y + y1; 90 y1 = b1 * y - a1 * z + y2; 91 y2 = b2 * y - a2 * z; 92 *dst = z * vol; 93 94 dst += dst_stride; 95 src += src_stride; 96 } 97 #define F(x) (std::isnormal(x) ? (x) : 0.0f) 98 lr4->x1 = F(x1); 99 lr4->x2 = F(x2); 100 lr4->y1 = F(y1); 101 lr4->y2 = F(y2); 102 #undef F 103 } 104 105 static void set_coefficient(struct biquad *bq, double b0, double b1, double b2, 106 double a0, double a1, double a2) 107 { 108 double a0_inv = 1 / a0; 109 bq->b0 = (float)(b0 * a0_inv); 110 bq->b1 = (float)(b1 * a0_inv); 111 bq->b2 = (float)(b2 * a0_inv); 112 bq->a1 = (float)(a1 * a0_inv); 113 bq->a2 = (float)(a2 * a0_inv); 114 } 115 116 /* Q = 1 / sqrt(2), also resulting Q value when S = 1 */ 117 #define BIQUAD_DEFAULT_Q 0.707106781186548 118 119 static void biquad_set_lowpass(struct biquad *bq, double cutoff, double Q) 120 { 121 /* Clear history values. */ 122 bq->x1 = 0; 123 bq->x2 = 0; 124 125 /* Limit cutoff to 0 to 1. */ 126 cutoff = fmax(0.0, fmin(cutoff, 1.0)); 127 128 if (cutoff == 1 || cutoff == 0) { 129 /* When cutoff is 1, the z-transform is 1. 130 * When cutoff is zero, nothing gets through the filter, so set 131 * coefficients up correctly. 132 */ 133 set_coefficient(bq, cutoff, 0, 0, 1, 0, 0); 134 return; 135 } 136 137 /* Set Q to a sane default value if not set */ 138 if (Q <= 0) 139 Q = BIQUAD_DEFAULT_Q; 140 141 /* Compute biquad coefficients for lowpass filter */ 142 /* H(s) = 1 / (s^2 + s/Q + 1) */ 143 double w0 = M_PI * cutoff; 144 double alpha = sin(w0) / (2 * Q); 145 double k = cos(w0); 146 147 double b0 = (1 - k) / 2; 148 double b1 = 1 - k; 149 double b2 = (1 - k) / 2; 150 double a0 = 1 + alpha; 151 double a1 = -2 * k; 152 double a2 = 1 - alpha; 153 154 set_coefficient(bq, b0, b1, b2, a0, a1, a2); 155 } 156 157 static void biquad_set_none(biquad* bq) 158 { 159 bq->x1 = 0; 160 bq->x2 = 0; 161 set_coefficient(bq, 1, 0, 0, 1, 0, 0); 162 } 163 164 static void lr4_set(struct lr4 *lr4, bool lowpass, float freq) 165 { 166 lowpass ? biquad_set_lowpass(&lr4->bq, freq, 0) : biquad_set_none(&lr4->bq); 167 lr4->x1 = 0; 168 lr4->x2 = 0; 169 lr4->y1 = 0; 170 lr4->y2 = 0; 171 lr4->active = lowpass; 172 } 173 174 void PWDecoder::DelayConvolveRun( 175 std::span<float> buffer, uint32_t *pos, 176 float *dst, std::size_t dst_stride, 177 const float *src, std::size_t src_stride, const float vol, 178 std::size_t n_samples) 179 { 180 uint32_t w = *pos; 181 auto n_buffer = uint32_t(buffer.size() / 2); 182 auto o = n_buffer - delay - taps.size()-1; 183 184 if (taps.size() == 1) { 185 for (std::size_t i = 0; i < n_samples; i++) { 186 buffer[w] = buffer[w + n_buffer] = *src; 187 *dst = buffer[w + o] * vol; 188 w = w + 1 >= n_buffer ? 0 : w + 1; 189 190 dst += dst_stride; 191 src += src_stride; 192 } 193 } else { 194 for (std::size_t i = 0; i < n_samples; i++) { 195 float sum = 0.0f; 196 197 buffer[w] = buffer[w + n_buffer] = *src; 198 for (std::size_t j = 0; j < taps.size(); j++) 199 sum += taps[j] * buffer[w+o+j]; 200 *dst = sum * vol; 201 202 w = w + 1 >= n_buffer ? 0 : w + 1; 203 dst += dst_stride; 204 src += src_stride; 205 } 206 } 207 *pos = w; 208 } 209 210 static inline void blackman_window(std::span<float> taps) 211 { 212 for (size_t n = 0; n < taps.size(); n++) { 213 float w = 2.0f * float(M_PI) * float(n) / float(taps.size()-1); 214 taps[n] = 0.3635819f - 0.4891775f * cosf(w) 215 + 0.1365995f * cosf(2 * w) - 0.0106411f * cosf(3 * w); 216 } 217 } 218 219 static inline void hilbert_generate(std::span<float> taps) 220 { 221 for (std::size_t i = 0; i < taps.size(); i++) { 222 int k = -int(taps.size() / 2) + int(i); 223 if (k & 1) { 224 float pk = (float)M_PI * k; 225 taps[i] *= (1.0f - cosf(pk)) / pk; 226 } else { 227 taps[i] = 0.0f; 228 } 229 } 230 } 231 232 static inline void reverse_taps(std::span<float> taps) 233 { 234 for (size_t i = 0; i < taps.size()/2; i++) 235 std::swap(taps[i], taps[taps.size()-1-i]); 236 } 237 238 239 static constexpr float SQRT1_2 = 0.707106781f; 240 241 void PWDecoder::Mix4(std::span<const float> src) 242 { 243 auto n_samples = src.size() / 2; 244 buf.resize(n_samples * 4); 245 Vol<2>(buf.data(), 4, src.data(), 2, n_samples, 1.f); 246 247 if (!psd) { 248 Vol<2>(buf.data() + 2, 4, src.data(), 2, n_samples, SQRT1_2); 249 } else { 250 Sub(buf.data() + 2, 4, src); 251 252 DelayConvolveRun(buffer[1], &pos[1], buf.data() + 3, 4, buf.data() + 2, 4, 253 -SQRT1_2, n_samples); 254 DelayConvolveRun(buffer[0], &pos[0], buf.data() + 2, 4, buf.data() + 2, 4, 255 SQRT1_2, n_samples); 256 } 257 } 258 259 void PWDecoder::Mix3p1Base(std::span<const float> src, std::size_t dst_stride) 260 { 261 auto n_samples = src.size() / 2; 262 263 const float v2 = SQRT1_2; 264 const float v3 = lfe_cutoff > 0 ? .5f : .0f; 265 266 if (widen == 0.0f) { 267 Vol<2>(buf.data(), dst_stride, src.data(), 2, n_samples, 1.f); 268 Avg(buf.data() + 2, dst_stride, src); 269 } else { 270 for (uint32_t n = 0; n < n_samples; n++) { 271 float c = src[2*n] + src[2*n+1]; 272 float w = c * widen; 273 buf[dst_stride*n+0] = (src[2*n+0] - w); 274 buf[dst_stride*n+1] = (src[2*n+1] - w); 275 buf[dst_stride*n+2] = c * 0.5f; 276 } 277 } 278 lr4_process_c(&lr4[1], buf.data() + 3, dst_stride, buf.data() + 2, dst_stride, 279 v3, n_samples); // lr3 280 lr4_process_c(&lr4[0], buf.data() + 2, dst_stride, buf.data() + 2, dst_stride, 281 v2, n_samples); // lr2 282 } 283 284 void PWDecoder::Mix3p1(std::span<const float> src) 285 { 286 buf.resize(src.size() * 2); 287 Mix3p1Base(src, 4); 288 } 289 290 void PWDecoder::Mix5p1(std::span<const float> src) 291 { 292 auto n_samples = src.size() / 2; 293 buf.resize(n_samples * 6); 294 Mix3p1Base(src, 6); 295 296 if (!psd) { 297 Vol<2>(buf.data() + 4, 6, src.data(), 2, n_samples, SQRT1_2); 298 } else { 299 Sub(buf.data() + 4, 6, src); 300 301 DelayConvolveRun(buffer[1], &pos[1], buf.data() + 5, 6, buf.data() + 4, 6, 302 -SQRT1_2, n_samples); 303 DelayConvolveRun(buffer[0], &pos[0], buf.data() + 4, 6, buf.data() + 4, 6, 304 SQRT1_2, n_samples); 305 } 306 } 307 308 void PWDecoder::Mix7p1(std::span<const float> src) 309 { 310 auto n_samples = src.size() / 2; 311 buf.resize(n_samples * 8); 312 Mix3p1Base(src, 8); 313 314 Vol<2>(buf.data() + 4, 8, src.data(), 2, n_samples, SQRT1_2); 315 316 if (!psd) { 317 Vol<2>(buf.data() + 6, 8, src.data(), 2, n_samples, SQRT1_2); 318 } else { 319 Sub(buf.data() + 6, 8, src); 320 321 DelayConvolveRun(buffer[1], &pos[1], buf.data() + 7, 8, buf.data() + 6, 8, 322 -SQRT1_2, n_samples); 323 DelayConvolveRun(buffer[0], &pos[0], buf.data() + 6, 8, buf.data() + 6, 8, 324 SQRT1_2, n_samples); 325 } 326 } 327 328 static constexpr std::size_t BUFFER_SIZE = 4096; 329 PWDecoder::PWDecoder() 330 { 331 buffer[0].resize(2 * BUFFER_SIZE); 332 buffer[1].resize(2 * BUFFER_SIZE); 333 } 334 335 void PWDecoder::Init(float rate) 336 { 337 if (rate < 0) return; 338 this->rate = rate; 339 delay = std::uint32_t(rear_delay_ms * rate / 1000.0f); 340 pos = {0, 0}; 341 std::fill(buffer[0].begin(), buffer[0].end(), 0); 342 std::fill(buffer[1].begin(), buffer[1].end(), 0); 343 344 if (n_taps <= 1) 345 { 346 n_taps = 1; 347 taps.assign({1.f}); 348 } 349 else 350 { 351 n_taps = std::clamp<std::uint32_t>(n_taps, 15, 255) | 1; 352 taps.resize(n_taps); 353 blackman_window(taps); 354 hilbert_generate(taps); 355 reverse_taps(taps); 356 } 357 358 if (delay + taps.size() > BUFFER_SIZE) 359 delay = std::uint32_t(BUFFER_SIZE - taps.size()); 360 361 if (channel_setup != ChannelSetup::QUAD) 362 { 363 lr4_set(&lr4[1], lfe_cutoff > 0, lfe_cutoff / rate); 364 lr4_set(&lr4[0], fc_cutoff > 0, fc_cutoff / rate); 365 } 366 } 367 368 std::vector<Option> PWDecoder::GetOptions() 369 { 370 return { 371 { 372 "channel_setup", "SETUP", 373 "Output channel setup. One of: quad, 3.1, 5.1, 7.1. Default: 5.1", 374 [this](std::string_view sv) 375 { 376 /**/ if (sv == "quad") channel_setup = ChannelSetup::QUAD; 377 else if (sv == "3.1") channel_setup = ChannelSetup::_3P1; 378 else if (sv == "5.1") channel_setup = ChannelSetup::_5P1; 379 else if (sv == "7.1") channel_setup = ChannelSetup::_7P1; 380 else throw std::runtime_error("Invalid channel setup " + std::string{sv}); 381 } 382 }, 383 { 384 "psd", "BOOL", 385 "Enable Passive Surround Decoding. The rear channels as produced from the front left and right ambient sound (the difference between the channels). A delay and optional phase shift are added to the rear signal to make the sound bigger. If disabled, front is just copied to rear.", 386 [this](std::string_view sv) { psd = FromString<bool>(sv); } 387 }, 388 { 389 "widen", "FLOAT", 390 "Subtracts some of the front center signal from the stereo channels. This moves the dialogs more to the center speaker and leaves the ambient sound in the stereo channels.\nOnly active when Front Center is produced.", 391 [this](std::string_view sv) { widen = FromString<float>(sv); } 392 }, 393 { 394 "hilbert_taps", "INT", 395 "This option will apply a 90 degree phase shift to the rear channels to improve specialization. Taps needs to be between 15 and 255 with more accurate results (and more CPU consumption) for higher values.\nThis is only active when the psd up-mix method is used. 0 to disable.", 396 [this](std::string_view sv) 397 { 398 n_taps = FromString<std::uint32_t>(sv); 399 Init(rate); 400 } 401 }, 402 { 403 "rear_delay", "FLOAT", 404 "Apply a delay in milliseconds when up-mixing the rear channels. This improves specialization of the sound. A typical delay of 12 milliseconds is the default.\nThis is only active when the psd up-mix method is used.", 405 [this](std::string_view sv) 406 { 407 rear_delay_ms = FromString<float>(sv); 408 Init(rate); 409 } 410 }, 411 { 412 "lfe_cutoff", "FLOAT", 413 "Apply a lowpass filter to the low frequency effects. The value is expressed in Hz. Typical subwoofers have a cutoff at around 150 and 200. 0 disables the feature. Default: 150", 414 [this](std::string_view sv) 415 { 416 lfe_cutoff = FromString<float>(sv); 417 Init(rate); 418 } 419 }, 420 { 421 "fc_cutoff", "FLOAT", 422 "Apply a lowpass filter to the front center frequency. The value is expressed in Hz.\nSince the front center contains the dialogs, a typical cutoff frequency is 12000 Hz.", 423 [this](std::string_view sv) 424 { 425 lfe_cutoff = FromString<float>(sv); 426 Init(rate); 427 } 428 }, 429 }; 430 } 431 432 std::vector<Channel::E> PWDecoder::GetChannels() const 433 { 434 using E = Channel::E; 435 switch (channel_setup) 436 { 437 case ChannelSetup::QUAD: 438 return { E::FRONT_LEFT, E::FRONT_RIGHT, E::REAR_LEFT, E::REAR_RIGHT }; 439 case ChannelSetup::_3P1: 440 return { E::FRONT_LEFT, E::FRONT_RIGHT, E::FRONT_CENTER, E::LFE }; 441 case ChannelSetup::_5P1: 442 return { E::FRONT_LEFT, E::FRONT_RIGHT, E::FRONT_CENTER, E::LFE, 443 E::REAR_LEFT, E::REAR_RIGHT }; 444 case ChannelSetup::_7P1: 445 return { E::FRONT_LEFT, E::FRONT_RIGHT, E::FRONT_CENTER, E::LFE, 446 E::SIDE_LEFT, E::SIDE_RIGHT, E::REAR_LEFT, E::REAR_RIGHT }; 447 } 448 } 449 450 std::span<const float> PWDecoder::Decode(std::span<const float> in) 451 { 452 switch (channel_setup) 453 { 454 case ChannelSetup::QUAD: Mix4(in); break; 455 case ChannelSetup::_3P1: Mix3p1(in); break; 456 case ChannelSetup::_5P1: Mix5p1(in); break; 457 case ChannelSetup::_7P1: Mix7p1(in); break; 458 } 459 return buf; 460 }