core/converter.cpp

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