Miroslav's speed optimization patch
[flac.git] / src / plugin_common / dither.c
1 /* plugin_common - Routines common to several plugins
2  * Copyright (C) 2002,2003  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 #if defined _MSC_VER
33 #define FLAC__INLINE __inline
34 #else
35 #define FLAC__INLINE
36 #endif
37
38 /* 32-bit pseudo-random number generator */
39 static FLAC__INLINE FLAC__uint32 prng(FLAC__uint32 state)
40 {
41         return (state * 0x0019660dL + 0x3c6ef35fL) & 0xffffffffL;
42 }
43
44 /* dither routine derived from MAD winamp plugin */
45
46 typedef struct {
47         FLAC__int32 error[3];
48         FLAC__int32 random;
49 } dither_state;
50
51 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)
52 {
53         unsigned scalebits;
54         FLAC__int32 output, mask, random;
55
56         FLAC__ASSERT(source_bps < 32);
57         FLAC__ASSERT(target_bps <= 24);
58         FLAC__ASSERT(target_bps <= source_bps);
59
60         /* noise shape */
61         sample += dither->error[0] - dither->error[1] + dither->error[2];
62
63         dither->error[2] = dither->error[1];
64         dither->error[1] = dither->error[0] / 2;
65
66         /* bias */
67         output = sample + (1L << (source_bps - target_bps - 1));
68
69         scalebits = source_bps - target_bps;
70         mask = (1L << scalebits) - 1;
71
72         /* dither */
73         random = (FLAC__int32)prng(dither->random);
74         output += (random & mask) - (dither->random & mask);
75
76         dither->random = random;
77
78         /* clip */
79         if(output > MAX) {
80                 output = MAX;
81
82                 if(sample > MAX)
83                         sample = MAX;
84         }
85         else if(output < MIN) {
86                 output = MIN;
87
88                 if(sample < MIN)
89                         sample = MIN;
90         }
91
92         /* quantize */
93         output &= ~mask;
94
95         /* error feedback */
96         dither->error[0] = sample - output;
97
98         /* scale */
99         return output >> scalebits;
100 }
101
102 unsigned FLAC__plugin_common__pack_pcm_signed_little_endian(FLAC__byte *data, const FLAC__int32 * const input[], unsigned wide_samples, unsigned channels, unsigned source_bps, unsigned target_bps)
103 {
104         static dither_state dither[FLAC_PLUGIN__MAX_SUPPORTED_CHANNELS];
105         FLAC__byte * const start = data;
106         FLAC__int32 sample;
107         const FLAC__int32 *input_;
108         unsigned samples, channel;
109         const unsigned bytes_per_sample = target_bps / 8;
110         unsigned inc = bytes_per_sample * channels;
111
112         FLAC__ASSERT(channels > 0 && channels <= FLAC_PLUGIN__MAX_SUPPORTED_CHANNELS);
113         FLAC__ASSERT(source_bps < 32);
114         FLAC__ASSERT(target_bps <= 24);
115         FLAC__ASSERT(target_bps <= source_bps);
116         FLAC__ASSERT((source_bps & 7) == 0);
117         FLAC__ASSERT((target_bps & 7) == 0);
118
119         if(source_bps != target_bps) {
120                 const FLAC__int32 MIN = -(1L << (source_bps - 1));
121                 const FLAC__int32 MAX = ~MIN; /*(1L << (source_bps-1)) - 1 */
122
123                 for(channel = 0; channel < channels; channel++) {
124                         
125                         samples = wide_samples;
126                         data = start + bytes_per_sample * channel;
127                         input_ = input[channel];
128
129                         while(samples--) {
130                                 sample = linear_dither(source_bps, target_bps, *input_++, &dither[channel], MIN, MAX);
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 += inc;
145                         }
146                 }
147         }
148         else {
149                 for(channel = 0; channel < channels; channel++) {
150                         samples = wide_samples;
151                         data = start + bytes_per_sample * channel;
152                         input_ = input[channel];
153
154                         while(samples--) {
155                                 sample = *input_++;
156
157                                 switch(target_bps) {
158                                         case 8:
159                                                 data[0] = sample ^ 0x80;
160                                                 break;
161                                         case 24:
162                                                 data[2] = (FLAC__byte)(sample >> 16);
163                                                 /* fall through */
164                                         case 16:
165                                                 data[1] = (FLAC__byte)(sample >> 8);
166                                                 data[0] = (FLAC__byte)sample;
167                                 }
168
169                                 data += inc;
170                         }
171                 }
172         }
173
174         return data - start;
175 }