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 "core/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
LoadSample(DevFmtType_t<DevFmtByte> val)35 template<> inline float LoadSample<DevFmtByte>(DevFmtType_t<DevFmtByte> val) noexcept
36 { return val * (1.0f/128.0f); }
LoadSample(DevFmtType_t<DevFmtShort> val)37 template<> inline float LoadSample<DevFmtShort>(DevFmtType_t<DevFmtShort> val) noexcept
38 { return val * (1.0f/32768.0f); }
LoadSample(DevFmtType_t<DevFmtInt> val)39 template<> inline float LoadSample<DevFmtInt>(DevFmtType_t<DevFmtInt> val) noexcept
40 { return static_cast<float>(val) * (1.0f/2147483648.0f); }
LoadSample(DevFmtType_t<DevFmtFloat> val)41 template<> inline float LoadSample<DevFmtFloat>(DevFmtType_t<DevFmtFloat> val) noexcept
42 { return val; }
43
LoadSample(DevFmtType_t<DevFmtUByte> val)44 template<> inline float LoadSample<DevFmtUByte>(DevFmtType_t<DevFmtUByte> val) noexcept
45 { return LoadSample<DevFmtByte>(static_cast<int8_t>(val - 128)); }
LoadSample(DevFmtType_t<DevFmtUShort> val)46 template<> inline float LoadSample<DevFmtUShort>(DevFmtType_t<DevFmtUShort> val) noexcept
47 { return LoadSample<DevFmtShort>(static_cast<int16_t>(val - 32768)); }
LoadSample(DevFmtType_t<DevFmtUInt> val)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>
LoadSampleArray(float * RESTRICT dst,const void * src,const size_t srcstep,const size_t samples)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
LoadSamples(float * dst,const void * src,const size_t srcstep,const DevFmtType srctype,const size_t samples)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
StoreSample(float val)83 template<> inline float StoreSample<DevFmtFloat>(float val) noexcept
84 { return val; }
StoreSample(float val)85 template<> inline int32_t StoreSample<DevFmtInt>(float val) noexcept
86 { return fastf2i(clampf(val*2147483648.0f, -2147483648.0f, 2147483520.0f)); }
StoreSample(float val)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))); }
StoreSample(float val)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. */
StoreSample(float val)93 template<> inline uint32_t StoreSample<DevFmtUInt>(float val) noexcept
94 { return static_cast<uint32_t>(StoreSample<DevFmtInt>(val)) + 2147483648u; }
StoreSample(float val)95 template<> inline uint16_t StoreSample<DevFmtUShort>(float val) noexcept
96 { return static_cast<uint16_t>(StoreSample<DevFmtShort>(val) + 32768); }
StoreSample(float val)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>
StoreSampleArray(void * dst,const float * RESTRICT src,const size_t dststep,const size_t samples)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
StoreSamples(void * dst,const float * src,const size_t dststep,const DevFmtType dsttype,const size_t samples)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>
Mono2Stereo(float * RESTRICT dst,const void * src,const size_t frames)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>
Multi2Mono(uint chanmask,const size_t step,const float scale,float * RESTRICT dst,const void * src,const size_t frames)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
CreateSampleConverter(DevFmtType srcType,DevFmtType dstType,size_t numchans,uint srcRate,uint dstRate,Resampler resampler)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
availableOut(uint srcframes) const187 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
convert(const void ** src,uint * srcframes,void * dst,uint dstframes)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
convert(const void * src,float * dst,uint frames) const338 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 }
372