1 /*
2 * Copyright 2003-2021 The Music Player Daemon Project
3 * http://www.musicpd.org
4 *
5 * This program is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 2 of the License, or
8 * (at your option) any later version.
9 *
10 * This program is distributed in the hope that it will be useful,
11 * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 * GNU General Public License for more details.
14 *
15 * You should have received a copy of the GNU General Public License along
16 * with this program; if not, write to the Free Software Foundation, Inc.,
17 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
18 */
19
20 #include "Dither.hxx"
21 #include "Prng.hxx"
22 #include "Traits.hxx"
23
24 template<typename T, T MIN, T MAX, unsigned scale_bits>
25 inline T
Dither(T sample)26 PcmDither::Dither(T sample) noexcept
27 {
28 constexpr T round = 1 << (scale_bits - 1);
29 constexpr T mask = (1 << scale_bits) - 1;
30
31 sample += error[0] - error[1] + error[2];
32
33 error[2] = error[1];
34 error[1] = error[0] / 2;
35
36 /* round */
37 T output = sample + round;
38
39 const T rnd = pcm_prng(random);
40 output += (rnd & mask) - (random & mask);
41
42 random = rnd;
43
44 /* clip */
45 if (output > MAX) {
46 output = MAX;
47
48 if (sample > MAX)
49 sample = MAX;
50 } else if (output < MIN) {
51 output = MIN;
52
53 if (sample < MIN)
54 sample = MIN;
55 }
56
57 output &= ~mask;
58
59 error[0] = sample - output;
60
61 return output >> scale_bits;
62 }
63
64 template<typename ST, unsigned SBITS, unsigned DBITS>
65 inline ST
DitherShift(ST sample)66 PcmDither::DitherShift(ST sample) noexcept
67 {
68 static_assert(sizeof(ST) * 8 > SBITS, "Source type too small");
69 static_assert(SBITS > DBITS, "Non-positive scale_bits");
70
71 static constexpr ST MIN = -(ST(1) << (SBITS - 1));
72 static constexpr ST MAX = (ST(1) << (SBITS - 1)) - 1;
73
74 return Dither<ST, MIN, MAX, SBITS - DBITS>(sample);
75 }
76
77 template<typename ST, typename DT>
78 inline typename DT::value_type
DitherConvert(typename ST::value_type sample)79 PcmDither::DitherConvert(typename ST::value_type sample) noexcept
80 {
81 static_assert(ST::BITS > DT::BITS,
82 "Sample formats cannot be dithered");
83
84 constexpr unsigned scale_bits = ST::BITS - DT::BITS;
85
86 return Dither<typename ST::sum_type, ST::MIN, ST::MAX,
87 scale_bits>(sample);
88 }
89
90 template<typename ST, typename DT>
91 inline void
DitherConvert(typename DT::pointer dest,typename ST::const_pointer src,typename ST::const_pointer src_end)92 PcmDither::DitherConvert(typename DT::pointer dest,
93 typename ST::const_pointer src,
94 typename ST::const_pointer src_end) noexcept
95 {
96 while (src < src_end)
97 *dest++ = DitherConvert<ST, DT>(*src++);
98 }
99
100 inline void
Dither24To16(int16_t * dest,const int32_t * src,const int32_t * src_end)101 PcmDither::Dither24To16(int16_t *dest, const int32_t *src,
102 const int32_t *src_end) noexcept
103 {
104 using ST = SampleTraits<SampleFormat::S24_P32>;
105 using DT = SampleTraits<SampleFormat::S16>;
106 DitherConvert<ST, DT>(dest, src, src_end);
107 }
108
109 inline void
Dither32To16(int16_t * dest,const int32_t * src,const int32_t * src_end)110 PcmDither::Dither32To16(int16_t *dest, const int32_t *src,
111 const int32_t *src_end) noexcept
112 {
113 using ST = SampleTraits<SampleFormat::S32>;
114 using DT = SampleTraits<SampleFormat::S16>;
115 DitherConvert<ST, DT>(dest, src, src_end);
116 }
117