text: /*
   * libnogg: a decoder library for Ogg Vorbis streams
   * Copyright (c) 2014-2019 Andrew Church <achurch@achurch.org>
   *
   * This software may be copied and redistributed under certain conditions;
   * see the file "COPYING" in the source code distribution for details.
   * NO WARRANTY is provided with this software.
   */
  
  #include "include/nogg.h"
  #include "src/common.h"
  #include "src/util/decode-frame.h"
  #include "src/util/float-to-int16.h"
  
  #include <string.h>
  
  #ifdef ENABLE_ASM_ARM_NEON
  # include <arm_neon.h>
  #endif
  #ifdef ENABLE_ASM_X86_SSE2
  # include <xmmintrin.h>
  #endif
  
  /*************************************************************************/
  /**************************** Helper routines ****************************/
  /*************************************************************************/
  
  /**
   * interleave:  Interleave source channels into a destination buffer.
   *
   * [Parameters]
   *     dest: Destination buffer pointer.
   *     src: Source buffer pointer array.
   *     channels: Number of channels.
   *     samples: Number of samples per channel.
   */
  static void interleave(float *dest, float **src, int channels, int samples)
  {
      for (int i = 0; i < samples; i++) {
          for (int c = 0; c < channels; c++) {
              *dest++ = src[c][i];
          }
      }
  }
  
  /*-----------------------------------------------------------------------*/
  
  /**
   * interleave_2:  Interleave two source channels into a destination buffer.
   * Specialization of interleave() for channels==2.
   *
   * [Parameters]
   *     dest: Destination buffer pointer.
   *     src: Source buffer pointer array.
   *     samples: Number of samples per channel.
   */
  static void interleave_2(float *dest, float **src, int samples)
  {
      const float *src0 = src[0];
      const float *src1 = src[1];
  
  #ifdef ENABLE_ASM_X86_SSE2
      for (; samples >= 4; src0 += 4, src1 += 4, dest += 8, samples -= 4) {
          const __m128i data0 = _mm_load_si128((const void *)src0);
          const __m128i data1 = _mm_load_si128((const void *)src1);
          _mm_store_si128((void *)dest, _mm_unpacklo_epi32(data0, data1));
          _mm_store_si128((void *)(dest+4), _mm_unpackhi_epi32(data0, data1));
      }
  #endif
  
  #ifdef ENABLE_ASM_ARM_NEON
      for (; samples >= 4; src0 += 4, src1 += 4, dest += 8, samples -= 4) {
          float32x4x2_t data;
          data.val[0] = vld1q_f32(src0);
          data.val[1] = vld1q_f32(src1);
          vst2q_f32(dest, data);
      }
  #endif  // ENABLE_ASM_ARM_NEON
  
      for (int i = 0; i < samples; i++) {
          dest[i*2+0] = src0[i];
          dest[i*2+1] = src1[i];
      }
  }
  
  /*************************************************************************/
  /************************** Interface routines ***************************/
  /*************************************************************************/
  
  vorbis_error_t decode_frame(vorbis_t *handle, const void *packet,
                              int32_t packet_len)
  {
      handle->frame_pos += handle->decode_buf_len;
      handle->decode_buf_pos = 0;
      handle->decode_buf_len = 0;
  
      (void) stb_vorbis_get_error(handle->decoder);  // Clear any pending error.
      float **outputs;
      int samples = 0;
      if (handle->packet_mode) {
          ASSERT(packet != NULL);
          ASSERT(packet_len > 0);
          (void) stb_vorbis_decode_packet_float(handle->decoder, packet,
                                                packet_len, &outputs, &samples);
      } else {
          do {
              stb_vorbis_reset_eof(handle->decoder);
              if (!stb_vorbis_get_frame_float(handle->decoder,
                                              &outputs, &samples)) {
                  break;
              }
              handle->frame_pos = stb_vorbis_tell_pcm(handle->decoder) - samples;
          } while (samples == 0);
      }
  
      if (samples > 0) {
          const int channels = handle->channels;
          if (handle->read_int16_only) {
              int16_t *decode_buf = handle->decode_buf;
              if (channels == 1) {
                  float_to_int16(decode_buf, outputs[0], samples);
              } else if (channels == 2) {
                  float_to_int16_interleave_2(decode_buf, outputs[0],
                                              outputs[1], samples);
              } else {
                  float_to_int16_interleave(decode_buf, outputs, channels,
                                            samples);
              }
          } else {
              float *decode_buf = handle->decode_buf;
              if (channels == 1) {
                  memcpy(decode_buf, outputs[0], sizeof(*decode_buf) * samples);
              } else if (channels == 2) {
                  interleave_2(decode_buf, outputs, samples);
              } else {
                  interleave(decode_buf, outputs, channels, samples);
              }
          }
      }
      handle->decode_buf_len = samples;
  
      const STBVorbisError stb_error = stb_vorbis_get_error(handle->decoder);
      if (samples == 0 && stb_error == VORBIS__no_error) {
          return VORBIS_ERROR_STREAM_END;
      } else if (stb_error == VORBIS_invalid_packet
              || stb_error == VORBIS_continued_packet_flag_invalid
              || stb_error == VORBIS_wrong_page_number) {
          return VORBIS_ERROR_DECODE_RECOVERED;
      } else if (stb_error != VORBIS__no_error) {
          return VORBIS_ERROR_DECODE_FAILED;
      } else {
          return VORBIS_NO_ERROR;
      }
  }
  
  /*************************************************************************/
  /*************************************************************************/
  