core/converter.cpp

   1
   2 #include "config.h"
   3
   4 #include "converter.h"
   5
   6 #include <algorithm>
   7 #include <cassert>
   8 #include <cmath>
   9 #include <cstdint>
  10 #include <iterator>
  11 #include <limits.h>
  12
  13 #include "albit.h"
  14 #include "albyte.h"
  15 #include "alnumeric.h"
  16 #include "fpu_ctrl.h"
  17
  18
  19 namespace {
  20
  21 constexpr uint MaxPitch{10};
  22
  23 static_assert((BufferLineSize-1)/MaxPitch > 0, "MaxPitch is too large for BufferLineSize!");
  24 static_assert((INT_MAX>>MixerFracBits)/MaxPitch > BufferLineSize,
  25     "MaxPitch and/or BufferLineSize are too large for MixerFracBits!");
  26
  27 /* Base template left undefined. Should be marked =delete, but Clang 3.8.1
  28  * chokes on that given the inline specializations.
  29  */
  30 template<DevFmtType T>
  31 inline float LoadSample(DevFmtType_t<T> val) noexcept;
  32
  33 template<> inline float LoadSample<DevFmtByte>(DevFmtType_t<DevFmtByte> val) noexcept
  34 { return val * (1.0f/128.0f); }
  35 template<> inline float LoadSample<DevFmtShort>(DevFmtType_t<DevFmtShort> val) noexcept
  36 { return val * (1.0f/32768.0f); }
  37 template<> inline float LoadSample<DevFmtInt>(DevFmtType_t<DevFmtInt> val) noexcept
  38 { return static_cast<float>(val) * (1.0f/2147483648.0f); }
  39 template<> inline float LoadSample<DevFmtFloat>(DevFmtType_t<DevFmtFloat> val) noexcept
  40 { return val; }
  41
  42 template<> inline float LoadSample<DevFmtUByte>(DevFmtType_t<DevFmtUByte> val) noexcept
  43 { return LoadSample<DevFmtByte>(static_cast<int8_t>(val - 128)); }
  44 template<> inline float LoadSample<DevFmtUShort>(DevFmtType_t<DevFmtUShort> val) noexcept
  45 { return LoadSample<DevFmtShort>(static_cast<int16_t>(val - 32768)); }
  46 template<> inline float LoadSample<DevFmtUInt>(DevFmtType_t<DevFmtUInt> val) noexcept
  47 { return LoadSample<DevFmtInt>(static_cast<int32_t>(val - 2147483648u)); }
  48
  49
  50 template<DevFmtType T>
  51 inline void LoadSampleArray(float *RESTRICT dst, const void *src, const size_t srcstep,
  52     const size_t samples) noexcept
  53 {
  54     const DevFmtType_t<T> *ssrc = static_cast<const DevFmtType_t<T>*>(src);
  55     for(size_t i{0u};i < samples;i++)
  56         dst[i] = LoadSample<T>(ssrc[i*srcstep]);
  57 }
  58
  59 void LoadSamples(float *dst, const void *src, const size_t srcstep, const DevFmtType srctype,
  60     const size_t samples) noexcept
  61 {
  62 #define HANDLE_FMT(T)                                                         \
  63     case T: LoadSampleArray<T>(dst, src, srcstep, samples); break
  64     switch(srctype)
  65     {
  66         HANDLE_FMT(DevFmtByte);
  67         HANDLE_FMT(DevFmtUByte);
  68         HANDLE_FMT(DevFmtShort);
  69         HANDLE_FMT(DevFmtUShort);
  70         HANDLE_FMT(DevFmtInt);
  71         HANDLE_FMT(DevFmtUInt);
  72         HANDLE_FMT(DevFmtFloat);
  73     }
  74 #undef HANDLE_FMT
  75 }
  76
  77
  78 template<DevFmtType T>
  79 inline DevFmtType_t<T> StoreSample(float) noexcept;
  80
  81 template<> inline float StoreSample<DevFmtFloat>(float val) noexcept
  82 { return val; }
  83 template<> inline int32_t StoreSample<DevFmtInt>(float val) noexcept
  84 { return fastf2i(clampf(val*2147483648.0f, -2147483648.0f, 2147483520.0f)); }
  85 template<> inline int16_t StoreSample<DevFmtShort>(float val) noexcept
  86 { return static_cast<int16_t>(fastf2i(clampf(val*32768.0f, -32768.0f, 32767.0f))); }
  87 template<> inline int8_t StoreSample<DevFmtByte>(float val) noexcept
  88 { return static_cast<int8_t>(fastf2i(clampf(val*128.0f, -128.0f, 127.0f))); }
  89
  90 /* Define unsigned output variations. */
  91 template<> inline uint32_t StoreSample<DevFmtUInt>(float val) noexcept
  92 { return static_cast<uint32_t>(StoreSample<DevFmtInt>(val)) + 2147483648u; }
  93 template<> inline uint16_t StoreSample<DevFmtUShort>(float val) noexcept
  94 { return static_cast<uint16_t>(StoreSample<DevFmtShort>(val) + 32768); }
  95 template<> inline uint8_t StoreSample<DevFmtUByte>(float val) noexcept
  96 { return static_cast<uint8_t>(StoreSample<DevFmtByte>(val) + 128); }
  97
  98 template<DevFmtType T>
  99 inline void StoreSampleArray(void *dst, const float *RESTRICT src, const size_t dststep,
 100     const size_t samples) noexcept
 101 {
 102     DevFmtType_t<T> *sdst = static_cast<DevFmtType_t<T>*>(dst);
 103     for(size_t i{0u};i < samples;i++)
 104         sdst[i*dststep] = StoreSample<T>(src[i]);
 105 }
 106
 107
 108 void StoreSamples(void *dst, const float *src, const size_t dststep, const DevFmtType dsttype,
 109     const size_t samples) noexcept
 110 {
 111 #define HANDLE_FMT(T)                                                         \
 112     case T: StoreSampleArray<T>(dst, src, dststep, samples); break
 113     switch(dsttype)
 114     {
 115         HANDLE_FMT(DevFmtByte);
 116         HANDLE_FMT(DevFmtUByte);
 117         HANDLE_FMT(DevFmtShort);
 118         HANDLE_FMT(DevFmtUShort);
 119         HANDLE_FMT(DevFmtInt);
 120         HANDLE_FMT(DevFmtUInt);
 121         HANDLE_FMT(DevFmtFloat);
 122     }
 123 #undef HANDLE_FMT
 124 }
 125
 126
 127 template<DevFmtType T>
 128 void Mono2Stereo(float *RESTRICT dst, const void *src, const size_t frames) noexcept
 129 {
 130     const DevFmtType_t<T> *ssrc = static_cast<const DevFmtType_t<T>*>(src);
 131     for(size_t i{0u};i < frames;i++)
 132         dst[i*2 + 1] = dst[i*2 + 0] = LoadSample<T>(ssrc[i]) * 0.707106781187f;
 133 }
 134
 135 template<DevFmtType T>
 136 void Multi2Mono(uint chanmask, const size_t step, const float scale, float *RESTRICT dst,
 137     const void *src, const size_t frames) noexcept
 138 {
 139     const DevFmtType_t<T> *ssrc = static_cast<const DevFmtType_t<T>*>(src);
 140     std::fill_n(dst, frames, 0.0f);
 141     for(size_t c{0};chanmask;++c)
 142     {
 143         if((chanmask&1)) LIKELY
 144         {
 145             for(size_t i{0u};i < frames;i++)
 146                 dst[i] += LoadSample<T>(ssrc[i*step + c]);
 147         }
 148         chanmask >>= 1;
 149     }
 150     for(size_t i{0u};i < frames;i++)
 151         dst[i] *= scale;
 152 }
 153
 154 } // namespace
 155
 156 SampleConverterPtr SampleConverter::Create(DevFmtType srcType, DevFmtType dstType, size_t numchans,
 157     uint srcRate, uint dstRate, Resampler resampler)
 158 {
 159     if(numchans < 1 || srcRate < 1 || dstRate < 1)
 160         return nullptr;
 161
 162     SampleConverterPtr converter{new(FamCount(numchans)) SampleConverter{numchans}};
 163     converter->mSrcType = srcType;
 164     converter->mDstType = dstType;
 165     converter->mSrcTypeSize = BytesFromDevFmt(srcType);
 166     converter->mDstTypeSize = BytesFromDevFmt(dstType);
 167
 168     converter->mSrcPrepCount = MaxResamplerPadding;
 169     converter->mFracOffset = 0;
 170     for(auto &chan : converter->mChan)
 171     {
 172         const al::span<float> buffer{chan.PrevSamples};
 173         std::fill(buffer.begin(), buffer.end(), 0.0f);
 174     }
 175
 176     /* Have to set the mixer FPU mode since that's what the resampler code expects. */
 177     FPUCtl mixer_mode{};
 178     auto step = static_cast<uint>(
 179         mind(srcRate*double{MixerFracOne}/dstRate + 0.5, MaxPitch*MixerFracOne));
 180     converter->mIncrement = maxu(step, 1);
 181     if(converter->mIncrement == MixerFracOne)
 182         converter->mResample = [](const InterpState*, const float *RESTRICT src, uint, const uint,
 183             const al::span<float> dst) { std::copy_n(src, dst.size(), dst.begin()); };
 184     else
 185         converter->mResample = PrepareResampler(resampler, converter->mIncrement,
 186             &converter->mState);
 187
 188     return converter;
 189 }
 190
 191 uint SampleConverter::availableOut(uint srcframes) const
 192 {
 193     if(srcframes < 1)
 194     {
 195         /* No output samples if there's no input samples. */
 196         return 0;
 197     }
 198
 199     const uint prepcount{mSrcPrepCount};
 200     if(prepcount < MaxResamplerPadding && MaxResamplerPadding - prepcount >= srcframes)
 201     {
 202         /* Not enough input samples to generate an output sample. */
 203         return 0;
 204     }
 205
 206     uint64_t DataSize64{prepcount};
 207     DataSize64 += srcframes;
 208     DataSize64 -= MaxResamplerPadding;
 209     DataSize64 <<= MixerFracBits;
 210     DataSize64 -= mFracOffset;
 211
 212     /* If we have a full prep, we can generate at least one sample. */
 213     return static_cast<uint>(clampu64((DataSize64 + mIncrement-1)/mIncrement, 1,
 214         std::numeric_limits<int>::max()));
 215 }
 216
 217 uint SampleConverter::convert(const void **src, uint *srcframes, void *dst, uint dstframes)
 218 {
 219     const uint SrcFrameSize{static_cast<uint>(mChan.size()) * mSrcTypeSize};
 220     const uint DstFrameSize{static_cast<uint>(mChan.size()) * mDstTypeSize};
 221     const uint increment{mIncrement};
 222     auto SamplesIn = static_cast<const al::byte*>(*src);
 223     uint NumSrcSamples{*srcframes};
 224
 225     FPUCtl mixer_mode{};
 226     uint pos{0};
 227     while(pos < dstframes && NumSrcSamples > 0)
 228     {
 229         const uint prepcount{mSrcPrepCount};
 230         const uint readable{minu(NumSrcSamples, BufferLineSize - prepcount)};
 231
 232         if(prepcount < MaxResamplerPadding && MaxResamplerPadding-prepcount >= readable)
 233         {
 234             /* Not enough input samples to generate an output sample. Store
 235              * what we're given for later.
 236              */
 237             for(size_t chan{0u};chan < mChan.size();chan++)
 238                 LoadSamples(&mChan[chan].PrevSamples[prepcount], SamplesIn + mSrcTypeSize*chan,
 239                     mChan.size(), mSrcType, readable);
 240
 241             mSrcPrepCount = prepcount + readable;
 242             NumSrcSamples = 0;
 243             break;
 244         }
 245
 246         float *RESTRICT SrcData{mSrcSamples};
 247         float *RESTRICT DstData{mDstSamples};
 248         uint DataPosFrac{mFracOffset};
 249         uint64_t DataSize64{prepcount};
 250         DataSize64 += readable;
 251         DataSize64 -= MaxResamplerPadding;
 252         DataSize64 <<= MixerFracBits;
 253         DataSize64 -= DataPosFrac;
 254
 255         /* If we have a full prep, we can generate at least one sample. */
 256         auto DstSize = static_cast<uint>(
 257             clampu64((DataSize64 + increment-1)/increment, 1, BufferLineSize));
 258         DstSize = minu(DstSize, dstframes-pos);
 259
 260         const uint DataPosEnd{DstSize*increment + DataPosFrac};
 261         const uint SrcDataEnd{DataPosEnd>>MixerFracBits};
 262
 263         assert(prepcount+readable >= SrcDataEnd);
 264         const uint nextprep{minu(prepcount + readable - SrcDataEnd, MaxResamplerPadding)};
 265
 266         for(size_t chan{0u};chan < mChan.size();chan++)
 267         {
 268             const al::byte *SrcSamples{SamplesIn + mSrcTypeSize*chan};
 269             al::byte *DstSamples = static_cast<al::byte*>(dst) + mDstTypeSize*chan;
 270
 271             /* Load the previous samples into the source data first, then the
 272              * new samples from the input buffer.
 273              */
 274             std::copy_n(mChan[chan].PrevSamples, prepcount, SrcData);
 275             LoadSamples(SrcData + prepcount, SrcSamples, mChan.size(), mSrcType, readable);
 276
 277             /* Store as many prep samples for next time as possible, given the
 278              * number of output samples being generated.
 279              */
 280             std::copy_n(SrcData+SrcDataEnd, nextprep, mChan[chan].PrevSamples);
 281             std::fill(std::begin(mChan[chan].PrevSamples)+nextprep,
 282                 std::end(mChan[chan].PrevSamples), 0.0f);
 283
 284             /* Now resample, and store the result in the output buffer. */
 285             mResample(&mState, SrcData+MaxResamplerEdge, DataPosFrac, increment,
 286                 {DstData, DstSize});
 287
 288             StoreSamples(DstSamples, DstData, mChan.size(), mDstType, DstSize);
 289         }
 290
 291         /* Update the number of prep samples still available, as well as the
 292          * fractional offset.
 293          */
 294         mSrcPrepCount = nextprep;
 295         mFracOffset = DataPosEnd & MixerFracMask;
 296
 297         /* Update the src and dst pointers in case there's still more to do. */
 298         const uint srcread{minu(NumSrcSamples, SrcDataEnd + mSrcPrepCount - prepcount)};
 299         SamplesIn += SrcFrameSize*srcread;
 300         NumSrcSamples -= srcread;
 301
 302         dst = static_cast<al::byte*>(dst) + DstFrameSize*DstSize;
 303         pos += DstSize;
 304     }
 305
 306     *src = SamplesIn;
 307     *srcframes = NumSrcSamples;
 308
 309     return pos;
 310 }
 311
 312
 313 void ChannelConverter::convert(const void *src, float *dst, uint frames) const
 314 {
 315     if(mDstChans == DevFmtMono)
 316     {
 317         const float scale{std::sqrt(1.0f / static_cast<float>(al::popcount(mChanMask)))};
 318         switch(mSrcType)
 319         {
 320 #define HANDLE_FMT(T) case T: Multi2Mono<T>(mChanMask, mSrcStep, scale, dst, src, frames); break
 321         HANDLE_FMT(DevFmtByte);
 322         HANDLE_FMT(DevFmtUByte);
 323         HANDLE_FMT(DevFmtShort);
 324         HANDLE_FMT(DevFmtUShort);
 325         HANDLE_FMT(DevFmtInt);
 326         HANDLE_FMT(DevFmtUInt);
 327         HANDLE_FMT(DevFmtFloat);
 328 #undef HANDLE_FMT
 329         }
 330     }
 331     else if(mChanMask == 0x1 && mDstChans == DevFmtStereo)
 332     {
 333         switch(mSrcType)
 334         {
 335 #define HANDLE_FMT(T) case T: Mono2Stereo<T>(dst, src, frames); break
 336         HANDLE_FMT(DevFmtByte);
 337         HANDLE_FMT(DevFmtUByte);
 338         HANDLE_FMT(DevFmtShort);
 339         HANDLE_FMT(DevFmtUShort);
 340         HANDLE_FMT(DevFmtInt);
 341         HANDLE_FMT(DevFmtUInt);
 342         HANDLE_FMT(DevFmtFloat);
 343 #undef HANDLE_FMT
 344         }
 345     }
 346 }