1 files changed, 154 insertions, 0 deletions
diff --git a/src/codecs/mad.cpp b/src/codecs/mad.cpp
new file mode 100644
index 00000000..f1ed0346
--- /dev/null
+++ b/src/codecs/mad.cpp
@@ -0,0 +1,154 @@
+#include "mad.hpp"
+#include <cstdint>
+
+#include "mad.h"
+
+namespace codecs {
+
+  static int32_t scaleTo24Bits(mad_fixed_t sample) {
+    // Round the bottom bits.
+    sample += (1L << (MAD_F_FRACBITS - 24));
+
+    // Clip the leftover bits to within range.
+    if (sample >= MAD_F_ONE)
+      sample = MAD_F_ONE - 1;
+    else if (sample < -MAD_F_ONE)
+      sample = -MAD_F_ONE;
+
+    /* quantize */
+    return sample >> (MAD_F_FRACBITS + 1 - 24);
+  }
+
+  MadMp3Decoder::MadMp3Decoder() {
+    mad_stream_init(&stream_);
+    mad_frame_init(&frame_);
+    mad_synth_init(&synth_);
+    mad_header_init(&header_);
+  }
+  MadMp3Decoder::~MadMp3Decoder() {
+    mad_stream_finish(&stream_);
+    mad_frame_finish(&frame_);
+    mad_synth_finish(&synth_);
+    mad_header_finish(&header_);
+  }
+
+      auto MadMp3Decoder::CanHandleExtension(std::string extension) -> bool {
+      return extension == "mp3";
+      }
+
+      auto GetOutputFormat() -> OutputFormat {
+        return OutputFormat {
+          .num_channels = synth_.pcm.channels,
+          .bits_per_sample = 24,
+          .sample_rate_hz = synth_.pcm.samplerate,
+        };
+      }
+
+      auto MadMp3Decoder::Process(
+          uint8_t *input,
+          std::size_t input_len,
+          uint8_t *output,
+	  std::size_t output_length) -> cpp::result<Result, Error> {
+
+        Result res {
+          .need_more_input = false,
+          .input_processed = 0,
+          .flush_output = false,
+          .output_written = 0,
+        }
+        while (true) {
+          // Only process more of the input if we're done sending off the
+          // samples for the previous frame.
+          if (current_sample_ == -1) {
+            ProcessInput(&res, input, input_len);
+          }
+
+          // Write PCM samples to the output buffer. This always needs to be
+          // done, even if we ran out of input, so that we don't keep the last
+          // few samples buffered if the input stream has actually finished.
+          WriteOutputSamples(&res, output, output_length);
+
+          if (res.need_more_input || res.flush_output) {
+            return res;
+          }
+      }
+
+      auto MadMp3Decoder::ProcessInput(
+          Result *res, uint8_t *input, std::size_t input_len) -> void {
+
+        if (input != stream_.buffer) {
+          mad_stream_buffer(&stream_, input, input_len);
+        }
+
+          if (!has_decoded_header_) {
+            // The header of any given frame should be representative of the
+            // entire stream, so only need to read it once.
+            mad_header_decode(&header_, &stream);
+            has_decoded_header_ = true;
+
+            // TODO: Use the info in the header for something. I think the
+            // duration will help with seeking?
+          }
+
+
+          // Decode the next frame. To signal errors, this returns -1 and
+          // stashes an error code in the stream structure.
+          if (mad_frame_decode(&frame_, &stream_) < 0) {
+            if (MAD_RECOVERABLE(stream_.error)) {
+              // Recoverable errors are usually malformed parts of the stream.
+              // We can recover from them by just retrying the decode.
+              continue;
+            }
+
+            if (stream_.error = MAD_ERROR_BUFLEN) {
+              // The decoder ran out of bytes before it completed a frame. We
+              // need to return back to the caller to give us more data. Note
+              // that there might still be some unused data in the input, so we
+              // should calculate that amount and return it.
+              size_t remaining_bytes = stream.bufend - stream_.next_frame;
+              return remaining_bytes;
+            }
+
+            // The error is unrecoverable. Give up.
+            return cpp::fail(MALFORMED_DATA);
+          }
+
+          // We've successfully decoded a frame!
+          // Now we need to synthesize PCM samples based on the frame, and send
+          // them downstream.
+          mad_synth_frame(&synth_, &frame_);
+          up_to_sample = 0;
+      }
+
+      auto MadMp3Decoder::WriteOutputSamples(
+          Result *res,
+          uint8_t *output,
+	  std::size_t output_length) -> void {
+        size_t output_byte = 0;
+        // First ensure that we actually have some samples to send off.
+        if (current_sample_ < 0) {
+          return;
+        }
+        res->flush_output = true;
+
+        while (current_sample_ < synth_.pcm.length) {
+          if (output_byte + (3 * synth_.pcm.channels) >= output_length) {
+            res->output_written = output_byte;
+            return;
+          }
+
+          for (int channel = 0; channel < synth_.pcm.channels; channel++) {
+            uint32_t sample_24 = scaleTo24Bits(synth_.pcm.samples[channel][sample]);
+            output[output_byte++] = (sample_24 >> 0) & 0xff;
+            output[output_byte++] = (sample_24 >> 8) & 0xff;
+            output[output_byte++] = (sample_24 >> 16) & 0xff;
+          }
+          current_sample_++;
+        }
+
+        // We wrote everything! Reset, ready for the next frame.
+        current_sample_ = -1;
+        res->output_written = output_byte;
+      }
+
+} // namespace codecs