initial import
[flac.git] / src / plugin_common / dither.c
1 /* plugin_common - Routines common to several plugins
2  * Copyright (C) 2002  Josh Coalson
3  *
4  * dithering routine derived from (other GPLed source):
5  * mad - MPEG audio decoder
6  * Copyright (C) 2000-2001 Robert Leslie
7  *
8  * This program is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * as published by the Free Software Foundation; either version 2
11  * of the License, or (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with this program; if not, write to the Free Software
20  * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
21  */
22
23 #include "dither.h"
24 #include "FLAC/assert.h"
25
26 #ifdef max
27 #undef max
28 #endif
29 #define max(a,b) ((a)>(b)?(a):(b))
30
31
32 #define FLAC__DO_DITHER
33
34 #define MAX_SUPPORTED_CHANNELS 2
35
36 #if defined _MSC_VER || defined __MINGW32__
37 #define FLAC__INLINE __inline
38 #else
39 #define FLAC__INLINE
40 #endif
41
42 /* 32-bit pseudo-random number generator */
43 static FLAC__INLINE FLAC__uint32 prng(FLAC__uint32 state)
44 {
45         return (state * 0x0019660dL + 0x3c6ef35fL) & 0xffffffffL;
46 }
47
48 /* dither routine derived from MAD winamp plugin */
49
50 typedef struct {
51         FLAC__int32 error[3];
52         FLAC__int32 random;
53 } dither_state;
54
55 static FLAC__INLINE FLAC__int32 linear_dither(unsigned source_bps, unsigned target_bps, FLAC__int32 sample, dither_state *dither, const FLAC__int32 MIN, const FLAC__int32 MAX)
56 {
57         unsigned scalebits;
58         FLAC__int32 output, mask, random;
59
60         FLAC__ASSERT(source_bps < 32);
61         FLAC__ASSERT(target_bps <= 24);
62         FLAC__ASSERT(target_bps <= source_bps);
63
64         /* noise shape */
65         sample += dither->error[0] - dither->error[1] + dither->error[2];
66
67         dither->error[2] = dither->error[1];
68         dither->error[1] = dither->error[0] / 2;
69
70         /* bias */
71         output = sample + (1L << (source_bps - target_bps - 1));
72
73         scalebits = source_bps - target_bps;
74         mask = (1L << scalebits) - 1;
75
76         /* dither */
77         random = (FLAC__int32)prng(dither->random);
78         output += (random & mask) - (dither->random & mask);
79
80         dither->random = random;
81
82         /* clip */
83         if(output > MAX) {
84                 output = MAX;
85
86                 if(sample > MAX)
87                         sample = MAX;
88         }
89         else if(output < MIN) {
90                 output = MIN;
91
92                 if(sample < MIN)
93                         sample = MIN;
94         }
95
96         /* quantize */
97         output &= ~mask;
98
99         /* error feedback */
100         dither->error[0] = sample - output;
101
102         /* scale */
103         return output >> scalebits;
104 }
105
106 unsigned FLAC__plugin_common__pack_pcm(FLAC__byte *data, FLAC__int32 *input, unsigned wide_samples, unsigned channels, unsigned source_bps, unsigned target_bps)
107 {
108         static dither_state dither[MAX_SUPPORTED_CHANNELS];
109         FLAC__byte * const start = data;
110         FLAC__int32 sample;
111         unsigned samples = wide_samples * channels;
112         const unsigned bytes_per_sample = target_bps / 8;
113
114         FLAC__ASSERT(MAX_SUPPORTED_CHANNELS == 2);
115         FLAC__ASSERT(channels > 0 && channels <= MAX_SUPPORTED_CHANNELS);
116         FLAC__ASSERT(source_bps < 32);
117         FLAC__ASSERT(target_bps <= 24);
118         FLAC__ASSERT(target_bps <= source_bps);
119         FLAC__ASSERT((source_bps & 7) == 0);
120         FLAC__ASSERT((target_bps & 7) == 0);
121
122         if(source_bps != target_bps) {
123                 const FLAC__int32 MIN = -(1L << source_bps);
124                 const FLAC__int32 MAX = ~MIN; /*(1L << (source_bps-1)) - 1 */
125                 const unsigned dither_twiggle = channels - 1;
126                 unsigned dither_source = 0;
127
128                 while(samples--) {
129                         sample = linear_dither(source_bps, target_bps, *input++, &dither[dither_source], MIN, MAX);
130                         dither_source ^= dither_twiggle;
131
132                         switch(target_bps) {
133                                 case 8:
134                                         data[0] = sample ^ 0x80;
135                                         break;
136                                 case 24:
137                                         data[2] = (FLAC__byte)(sample >> 16);
138                                         /* fall through */
139                                 case 16:
140                                         data[1] = (FLAC__byte)(sample >> 8);
141                                         data[0] = (FLAC__byte)sample;
142                         }
143
144                         data += bytes_per_sample;
145                 }
146         }
147         else {
148                 while(samples--) {
149                         sample = *input++;
150
151                         switch(target_bps) {
152                                 case 8:
153                                         data[0] = sample ^ 0x80;
154                                         break;
155                                 case 24:
156                                         data[2] = (FLAC__byte)(sample >> 16);
157                                         /* fall through */
158                                 case 16:
159                                         data[1] = (FLAC__byte)(sample >> 8);
160                                         data[0] = (FLAC__byte)sample;
161                         }
162
163                         data += bytes_per_sample;
164                 }
165         }
166
167         return data - start;
168 }