5 files changed, 317 insertions, 10 deletions

diff --git a/src/audio_core/CMakeLists.txt b/src/audio_core/CMakeLists.txt
index eba0a5697..a72a907ef 100644
--- a/src/audio_core/CMakeLists.txt
+++ b/src/audio_core/CMakeLists.txt
@@ -3,6 +3,7 @@ set(SRCS
             codec.cpp
             hle/dsp.cpp
             hle/filter.cpp
+            hle/mixers.cpp
             hle/pipe.cpp
             hle/source.cpp
             interpolate.cpp
@@ -16,6 +17,7 @@ set(HEADERS
             hle/common.h
             hle/dsp.h
             hle/filter.h
+            hle/mixers.h
             hle/pipe.h
             hle/source.h
             interpolate.h
diff --git a/src/audio_core/hle/dsp.cpp b/src/audio_core/hle/dsp.cpp
index 5113ad8ca..0640e1eff 100644
--- a/src/audio_core/hle/dsp.cpp
+++ b/src/audio_core/hle/dsp.cpp
@@ -6,6 +6,7 @@
 #include <memory>
 
 #include "audio_core/hle/dsp.h"
+#include "audio_core/hle/mixers.h"
 #include "audio_core/hle/pipe.h"
 #include "audio_core/hle/source.h"
 #include "audio_core/sink.h"
@@ -14,6 +15,8 @@
 namespace DSP {
 namespace HLE {
 
+// Region management
+
 std::array<SharedMemory, 2> g_regions;
 
 static size_t CurrentRegionIndex() {
@@ -41,16 +44,57 @@ static SharedMemory& WriteRegion() {
     return g_regions[1 - CurrentRegionIndex()];
 }
 
+// Audio processing and mixing
+
 static std::array<Source, num_sources> sources = {
     Source(0), Source(1), Source(2), Source(3), Source(4), Source(5),
     Source(6), Source(7), Source(8), Source(9), Source(10), Source(11),
     Source(12), Source(13), Source(14), Source(15), Source(16), Source(17),
     Source(18), Source(19), Source(20), Source(21), Source(22), Source(23)
 };
+static Mixers mixers;
+
+static StereoFrame16 GenerateCurrentFrame() {
+    SharedMemory& read = ReadRegion();
+    SharedMemory& write = WriteRegion();
+
+    std::array<QuadFrame32, 3> intermediate_mixes = {};
+
+    // Generate intermediate mixes
+    for (size_t i = 0; i < num_sources; i++) {
+        write.source_statuses.status[i] = sources[i].Tick(read.source_configurations.config[i], read.adpcm_coefficients.coeff[i]);
+        for (size_t mix = 0; mix < 3; mix++) {
+            sources[i].MixInto(intermediate_mixes[mix], mix);
+        }
+    }
+
+    // Generate final mix
+    write.dsp_status = mixers.Tick(read.dsp_configuration, read.intermediate_mix_samples, write.intermediate_mix_samples, intermediate_mixes);
+
+    StereoFrame16 output_frame = mixers.GetOutput();
+
+    // Write current output frame to the shared memory region
+    for (size_t samplei = 0; samplei < output_frame.size(); samplei++) {
+        for (size_t channeli = 0; channeli < output_frame[0].size(); channeli++) {
+            write.final_samples.pcm16[samplei][channeli] = s16_le(output_frame[samplei][channeli]);
+        }
+    }
+
+    return output_frame;
+}
+
+// Audio output
 
 static std::unique_ptr<AudioCore::Sink> sink;
 static AudioCore::TimeStretcher time_stretcher;
 
+static void OutputCurrentFrame(const StereoFrame16& frame) {
+    time_stretcher.AddSamples(&frame[0][0], frame.size());
+    sink->EnqueueSamples(time_stretcher.Process(sink->SamplesInQueue()));
+}
+
+// Public Interface
+
 void Init() {
     DSP::HLE::ResetPipes();
 
@@ -58,6 +102,8 @@ void Init() {
         source.Reset();
     }
 
+    mixers.Reset();
+
     time_stretcher.Reset();
     if (sink) {
         time_stretcher.SetOutputSampleRate(sink->GetNativeSampleRate());
@@ -75,17 +121,12 @@ void Shutdown() {
 }
 
 bool Tick() {
-    SharedMemory& read = ReadRegion();
-    SharedMemory& write = WriteRegion();
+    StereoFrame16 current_frame = {};
 
-    std::array<QuadFrame32, 3> intermediate_mixes = {};
+    // TODO: Check dsp::DSP semaphore (which indicates emulated application has finished writing to shared memory region)
+    current_frame = GenerateCurrentFrame();
 
-    for (size_t i = 0; i < num_sources; i++) {
-        write.source_statuses.status[i] = sources[i].Tick(read.source_configurations.config[i], read.adpcm_coefficients.coeff[i]);
-        for (size_t mix = 0; mix < 3; mix++) {
-            sources[i].MixInto(intermediate_mixes[mix], mix);
-        }
-    }
+    OutputCurrentFrame(current_frame);
 
     return true;
 }
diff --git a/src/audio_core/hle/dsp.h b/src/audio_core/hle/dsp.h
index f6e53f68f..9275cd7de 100644
--- a/src/audio_core/hle/dsp.h
+++ b/src/audio_core/hle/dsp.h
@@ -428,7 +428,7 @@ ASSERT_DSP_STRUCT(DspStatus, 32);
 /// Final mixed output in PCM16 stereo format, what you hear out of the speakers.
 /// When the application writes to this region it has no effect.
 struct FinalMixSamples {
-    s16_le pcm16[2 * samples_per_frame];
+    s16_le pcm16[samples_per_frame][2];
 };
 ASSERT_DSP_STRUCT(FinalMixSamples, 640);
 
diff --git a/src/audio_core/hle/mixers.cpp b/src/audio_core/hle/mixers.cpp
new file mode 100644
index 000000000..18335f7f0
--- /dev/null
+++ b/src/audio_core/hle/mixers.cpp
@@ -0,0 +1,201 @@
+// Copyright 2016 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#include <cstddef>
+
+#include "audio_core/hle/common.h"
+#include "audio_core/hle/dsp.h"
+#include "audio_core/hle/mixers.h"
+
+#include "common/assert.h"
+#include "common/logging/log.h"
+#include "common/math_util.h"
+
+namespace DSP {
+namespace HLE {
+
+void Mixers::Reset() {
+    current_frame.fill({});
+    state = {};
+}
+
+DspStatus Mixers::Tick(DspConfiguration& config,
+        const IntermediateMixSamples& read_samples,
+        IntermediateMixSamples& write_samples,
+        const std::array<QuadFrame32, 3>& input)
+{
+    ParseConfig(config);
+
+    AuxReturn(read_samples);
+    AuxSend(write_samples, input);
+
+    MixCurrentFrame();
+
+    return GetCurrentStatus();
+}
+
+void Mixers::ParseConfig(DspConfiguration& config) {
+    if (!config.dirty_raw) {
+        return;
+    }
+
+    if (config.mixer1_enabled_dirty) {
+        config.mixer1_enabled_dirty.Assign(0);
+        state.mixer1_enabled = config.mixer1_enabled != 0;
+        LOG_TRACE(Audio_DSP, "mixers mixer1_enabled = %hu", config.mixer1_enabled);
+    }
+
+    if (config.mixer2_enabled_dirty) {
+        config.mixer2_enabled_dirty.Assign(0);
+        state.mixer2_enabled = config.mixer2_enabled != 0;
+        LOG_TRACE(Audio_DSP, "mixers mixer2_enabled = %hu", config.mixer2_enabled);
+    }
+
+    if (config.volume_0_dirty) {
+        config.volume_0_dirty.Assign(0);
+        state.intermediate_mixer_volume[0] = config.volume[0];
+        LOG_TRACE(Audio_DSP, "mixers volume[0] = %f", config.volume[0]);
+    }
+
+    if (config.volume_1_dirty) {
+        config.volume_1_dirty.Assign(0);
+        state.intermediate_mixer_volume[1] = config.volume[1];
+        LOG_TRACE(Audio_DSP, "mixers volume[1] = %f", config.volume[1]);
+    }
+
+    if (config.volume_2_dirty) {
+        config.volume_2_dirty.Assign(0);
+        state.intermediate_mixer_volume[2] = config.volume[2];
+        LOG_TRACE(Audio_DSP, "mixers volume[2] = %f", config.volume[2]);
+    }
+
+    if (config.output_format_dirty) {
+        config.output_format_dirty.Assign(0);
+        state.output_format = config.output_format;
+        LOG_TRACE(Audio_DSP, "mixers output_format = %zu", static_cast<size_t>(config.output_format));
+    }
+
+    if (config.headphones_connected_dirty) {
+        config.headphones_connected_dirty.Assign(0);
+        // Do nothing.
+        // (Note: Whether headphones are connected does affect coefficients used for surround sound.)
+        LOG_TRACE(Audio_DSP, "mixers headphones_connected=%hu", config.headphones_connected);
+    }
+
+    if (config.dirty_raw) {
+        LOG_DEBUG(Audio_DSP, "mixers remaining_dirty=%x", config.dirty_raw);
+    }
+
+    config.dirty_raw = 0;
+}
+
+static s16 ClampToS16(s32 value) {
+    return static_cast<s16>(MathUtil::Clamp(value, -32768, 32767));
+}
+
+static std::array<s16, 2> AddAndClampToS16(const std::array<s16, 2>& a, const std::array<s16, 2>& b) {
+    return {
+        ClampToS16(static_cast<s32>(a[0]) + static_cast<s32>(b[0])),
+        ClampToS16(static_cast<s32>(a[1]) + static_cast<s32>(b[1]))
+    };
+}
+
+void Mixers::DownmixAndMixIntoCurrentFrame(float gain, const QuadFrame32& samples) {
+    // TODO(merry): Limiter. (Currently we're performing final mixing assuming a disabled limiter.)
+
+    switch (state.output_format) {
+    case OutputFormat::Mono:
+        std::transform(current_frame.begin(), current_frame.end(), samples.begin(), current_frame.begin(),
+            [gain](const std::array<s16, 2>& accumulator, const std::array<s32, 4>& sample) -> std::array<s16, 2> {
+                // Downmix to mono
+                s16 mono = ClampToS16(static_cast<s32>((gain * sample[0] + gain * sample[1] + gain * sample[2] + gain * sample[3]) / 2));
+                // Mix into current frame
+                return AddAndClampToS16(accumulator, { mono, mono });
+            });
+        return;
+
+    case OutputFormat::Surround:
+        // TODO(merry): Implement surround sound.
+        // fallthrough
+
+    case OutputFormat::Stereo:
+        std::transform(current_frame.begin(), current_frame.end(), samples.begin(), current_frame.begin(),
+            [gain](const std::array<s16, 2>& accumulator, const std::array<s32, 4>& sample) -> std::array<s16, 2> {
+                // Downmix to stereo
+                s16 left = ClampToS16(static_cast<s32>(gain * sample[0] + gain * sample[2]));
+                s16 right = ClampToS16(static_cast<s32>(gain * sample[1] + gain * sample[3]));
+                // Mix into current frame
+                return AddAndClampToS16(accumulator, { left, right });
+            });
+        return;
+    }
+
+    UNREACHABLE_MSG("Invalid output_format %zu", static_cast<size_t>(state.output_format));
+}
+
+void Mixers::AuxReturn(const IntermediateMixSamples& read_samples) {
+    // NOTE: read_samples.mix{1,2}.pcm32 annoyingly have their dimensions in reverse order to QuadFrame32.
+
+    if (state.mixer1_enabled) {
+        for (size_t sample = 0; sample < samples_per_frame; sample++) {
+            for (size_t channel = 0; channel < 4; channel++) {
+                state.intermediate_mix_buffer[1][sample][channel] = read_samples.mix1.pcm32[channel][sample];
+            }
+        }
+    }
+
+    if (state.mixer2_enabled) {
+        for (size_t sample = 0; sample < samples_per_frame; sample++) {
+            for (size_t channel = 0; channel < 4; channel++) {
+                state.intermediate_mix_buffer[2][sample][channel] = read_samples.mix2.pcm32[channel][sample];
+            }
+        }
+    }
+}
+
+void Mixers::AuxSend(IntermediateMixSamples& write_samples, const std::array<QuadFrame32, 3>& input) {
+    // NOTE: read_samples.mix{1,2}.pcm32 annoyingly have their dimensions in reverse order to QuadFrame32.
+
+    state.intermediate_mix_buffer[0] = input[0];
+
+    if (state.mixer1_enabled) {
+        for (size_t sample = 0; sample < samples_per_frame; sample++) {
+            for (size_t channel = 0; channel < 4; channel++) {
+                write_samples.mix1.pcm32[channel][sample] = input[1][sample][channel];
+            }
+        }
+    } else {
+        state.intermediate_mix_buffer[1] = input[1];
+    }
+
+    if (state.mixer2_enabled) {
+        for (size_t sample = 0; sample < samples_per_frame; sample++) {
+            for (size_t channel = 0; channel < 4; channel++) {
+                write_samples.mix2.pcm32[channel][sample] = input[2][sample][channel];
+            }
+        }
+    } else {
+        state.intermediate_mix_buffer[2] = input[2];
+    }
+}
+
+void Mixers::MixCurrentFrame() {
+    current_frame.fill({});
+
+    for (size_t mix = 0; mix < 3; mix++) {
+        DownmixAndMixIntoCurrentFrame(state.intermediate_mixer_volume[mix], state.intermediate_mix_buffer[mix]);
+    }
+
+    // TODO(merry): Compressor. (We currently assume a disabled compressor.)
+}
+
+DspStatus Mixers::GetCurrentStatus() const {
+    DspStatus status;
+    status.unknown = 0;
+    status.dropped_frames = 0;
+    return status;
+}
+
+} // namespace HLE
+} // namespace DSP
diff --git a/src/audio_core/hle/mixers.h b/src/audio_core/hle/mixers.h
new file mode 100644
index 000000000..b52952eb5
--- /dev/null
+++ b/src/audio_core/hle/mixers.h
@@ -0,0 +1,63 @@
+// Copyright 2016 Citra Emulator Project
+// Licensed under GPLv2 or any later version
+// Refer to the license.txt file included.
+
+#pragma once
+
+#include <array>
+
+#include "audio_core/hle/common.h"
+#include "audio_core/hle/dsp.h"
+
+namespace DSP {
+namespace HLE {
+
+class Mixers final {
+public:
+    Mixers() {
+        Reset();
+    }
+
+    void Reset();
+
+    DspStatus Tick(DspConfiguration& config,
+                   const IntermediateMixSamples& read_samples,
+                   IntermediateMixSamples& write_samples,
+                   const std::array<QuadFrame32, 3>& input);
+
+    StereoFrame16 GetOutput() const {
+        return current_frame;
+    }
+
+private:
+    StereoFrame16 current_frame = {};
+
+    using OutputFormat = DspConfiguration::OutputFormat;
+
+    struct {
+        std::array<float, 3> intermediate_mixer_volume = {};
+
+        bool mixer1_enabled = false;
+        bool mixer2_enabled = false;
+        std::array<QuadFrame32, 3> intermediate_mix_buffer = {};
+
+        OutputFormat output_format = OutputFormat::Stereo;
+
+    } state;
+
+    /// INTERNAL: Update our internal state based on the current config.
+    void ParseConfig(DspConfiguration& config);
+    /// INTERNAL: Read samples from shared memory that have been modified by the ARM11.
+    void AuxReturn(const IntermediateMixSamples& read_samples);
+    /// INTERNAL: Write samples to shared memory for the ARM11 to modify.
+    void AuxSend(IntermediateMixSamples& write_samples, const std::array<QuadFrame32, 3>& input);
+    /// INTERNAL: Mix current_frame.
+    void MixCurrentFrame();
+    /// INTERNAL: Downmix from quadraphonic to stereo based on status.output_format and accumulate into current_frame.
+    void DownmixAndMixIntoCurrentFrame(float gain, const QuadFrame32& samples);
+    /// INTERNAL: Generate DspStatus based on internal state.
+    DspStatus GetCurrentStatus() const;
+};
+
+} // namespace HLE
+} // namespace DSP