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