Ambisonic limiter, with gain linking between all channels to preserve imaging

libraries/audio/src/AudioLimiter.cpp

@@ -211,6 +211,49 @@ static inline int32_t peaklog2(float* input0, float* input1) {
     return (e << LOG2_FRACBITS) - (c2 >> 3);
 }
 
+//
+// Peak detection and -log2(x) for float input (quad)
+// x < 2^(31-LOG2_HEADROOM) returns 0x7fffffff
+// x > 2^LOG2_HEADROOM undefined
+//
+static inline int32_t peaklog2(float* input0, float* input1, float* input2, float* input3) {
+
+    // float as integer bits
+    int32_t u0 = *(int32_t*)input0;
+    int32_t u1 = *(int32_t*)input1;
+    int32_t u2 = *(int32_t*)input2;
+    int32_t u3 = *(int32_t*)input3;
+
+    // max absolute value
+    u0 &= IEEE754_FABS_MASK;
+    u1 &= IEEE754_FABS_MASK;
+    u2 &= IEEE754_FABS_MASK;
+    u3 &= IEEE754_FABS_MASK;
+    int32_t peak = MAX(MAX(u0, u1), MAX(u2, u3));
+
+    // split into e and x - 1.0
+    int32_t e = IEEE754_EXPN_BIAS - (peak >> IEEE754_MANT_BITS) + LOG2_HEADROOM;
+    int32_t x = (peak << (31 - IEEE754_MANT_BITS)) & 0x7fffffff;
+
+    // saturate
+    if (e > 31) {
+        return 0x7fffffff;
+    }
+
+    int k = x >> (31 - LOG2_TABBITS);
+
+    // polynomial for log2(1+x) over x=[0,1]
+    int32_t c0 = log2Table[k][0];
+    int32_t c1 = log2Table[k][1];
+    int32_t c2 = log2Table[k][2];
+
+    c1 += MULHI(c0, x);
+    c2 += MULHI(c1, x);
+
+    // reconstruct result in Q26
+    return (e << LOG2_FRACBITS) - (c2 >> 3);
+}
+
 //
 // Compute exp2(-x) for x=[0,32] in Q26, result in Q31
 // x < 0 undefined
@@ -376,6 +419,39 @@ public:
     }
 };
 
+//
+// N-1 sample delay (quad)
+//
+template<int N>
+class QuadDelay {
+
+    static_assert((N & (N - 1)) == 0, "N must be a power of 2");
+
+    float _buffer[4*N] = {};
+    int _index = 0;
+
+public:
+    void process(float& x0, float& x1, float& x2, float& x3) {
+
+        const int MASK = 4*N - 1;   // buffer wrap
+        int i = _index;
+
+        _buffer[i+0] = x0;
+        _buffer[i+1] = x1;
+        _buffer[i+2] = x2;
+        _buffer[i+3] = x3;
+
+        i = (i + 4*(N - 1)) & MASK;
+
+        x0 = _buffer[i+0];
+        x1 = _buffer[i+1];
+        x2 = _buffer[i+2];
+        x3 = _buffer[i+3];
+
+        _index = i;
+    }
+};
+
 //
 // Limiter (common)
 //
@@ -428,7 +504,7 @@ LimiterImpl::LimiterImpl(int sampleRate) {
 //
 void LimiterImpl::setThreshold(float threshold) {
 
-    const double OUT_CEILING = -0.3;
+    const double OUT_CEILING = -0.3;    // cannot be 0.0, due to dither
     const double Q31_TO_Q15 = 32768 / 2147483648.0;
 
     // limiter threshold = -48dB to 0dB
@@ -571,8 +647,8 @@ public:
 };
 
 template<int N>
-void LimiterMono<N>::process(float* input, int16_t* output, int numFrames)
-{
+void LimiterMono<N>::process(float* input, int16_t* output, int numFrames) {
+
     for (int n = 0; n < numFrames; n++) {
 
         // peak detect and convert to log2 domain
@@ -623,8 +699,8 @@ public:
 };
 
 template<int N>
-void LimiterStereo<N>::process(float* input, int16_t* output, int numFrames)
-{
+void LimiterStereo<N>::process(float* input, int16_t* output, int numFrames) {
+
     for (int n = 0; n < numFrames; n++) {
 
         // peak detect and convert to log2 domain
@@ -663,6 +739,71 @@ void LimiterStereo<N>::process(float* input, int16_t* output, int numFrames)
     }
 }
 
+//
+// Limiter (quad)
+//
+template<int N>
+class LimiterQuad : public LimiterImpl {
+
+    PeakFilter<N> _filter;
+    QuadDelay<N> _delay;
+
+public:
+    LimiterQuad(int sampleRate) : LimiterImpl(sampleRate) {}
+
+    // interleaved quad input/output
+    void process(float* input, int16_t* output, int numFrames) override;
+};
+
+template<int N>
+void LimiterQuad<N>::process(float* input, int16_t* output, int numFrames) {
+
+    for (int n = 0; n < numFrames; n++) {
+
+        // peak detect and convert to log2 domain
+        int32_t peak = peaklog2(&input[4*n+0], &input[4*n+1], &input[4*n+2], &input[4*n+3]);
+
+        // compute limiter attenuation
+        int32_t attn = MAX(_threshold - peak, 0);
+
+        // apply envelope
+        attn = envelope(attn);
+
+        // convert from log2 domain
+        attn = fixexp2(attn);
+
+        // lowpass filter
+        attn = _filter.process(attn);
+        float gain = attn * _outGain;
+
+        // delay audio
+        float x0 = input[4*n+0];
+        float x1 = input[4*n+1];
+        float x2 = input[4*n+2];
+        float x3 = input[4*n+3];
+        _delay.process(x0, x1, x2, x3);
+
+        // apply gain
+        x0 *= gain;
+        x1 *= gain;
+        x2 *= gain;
+        x3 *= gain;
+
+        // apply dither
+        float d = dither();
+        x0 += d;
+        x1 += d;
+        x2 += d;
+        x3 += d;
+
+        // store 16-bit output
+        output[4*n+0] = (int16_t)floatToInt(x0);
+        output[4*n+1] = (int16_t)floatToInt(x1);
+        output[4*n+2] = (int16_t)floatToInt(x2);
+        output[4*n+3] = (int16_t)floatToInt(x3);
+    }
+}
+
 //
 // Public API
 //
@@ -695,6 +836,19 @@ AudioLimiter::AudioLimiter(int sampleRate, int numChannels) {
             _impl = new LimiterStereo<128>(sampleRate);
         }
 
+    } else if (numChannels == 4) {
+
+        // ~1.5ms lookahead for all rates
+        if (sampleRate < 16000) {
+            _impl = new LimiterQuad<16>(sampleRate);
+        } else if (sampleRate < 32000) {
+            _impl = new LimiterQuad<32>(sampleRate);
+        } else if (sampleRate < 64000) {
+            _impl = new LimiterQuad<64>(sampleRate);
+        } else {
+            _impl = new LimiterQuad<128>(sampleRate);
+        }
+
     } else {
         assert(0); // unsupported
     }