trying some ideas for soft-decision DTD based on residual-to-signal ratio
[speexdsp.git] / libspeex / bits.c
1 /* Copyright (C) 2002 Jean-Marc Valin 
2    File: speex_bits.c
3
4    Handles bit packing/unpacking
5
6    Redistribution and use in source and binary forms, with or without
7    modification, are permitted provided that the following conditions
8    are met:
9    
10    - Redistributions of source code must retain the above copyright
11    notice, this list of conditions and the following disclaimer.
12    
13    - Redistributions in binary form must reproduce the above copyright
14    notice, this list of conditions and the following disclaimer in the
15    documentation and/or other materials provided with the distribution.
16    
17    - Neither the name of the Xiph.org Foundation nor the names of its
18    contributors may be used to endorse or promote products derived from
19    this software without specific prior written permission.
20    
21    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
24    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
25    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
26    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
27    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
28    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
29    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
30    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
31    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
32
33 */
34
35 #ifdef HAVE_CONFIG_H
36 #include "config.h"
37 #endif
38
39 #include <speex/speex_bits.h>
40 #include "misc.h"
41
42 /** Maximum size of the bit-stream (for fixed-size allocation) */
43 #define MAX_BYTES_PER_FRAME (2000/BYTES_PER_CHAR)
44
45 void speex_bits_init(SpeexBits *bits)
46 {
47    bits->chars = (char*)speex_alloc(MAX_BYTES_PER_FRAME);
48    bits->buf_size = MAX_BYTES_PER_FRAME;
49
50    bits->chars[0]=0;
51    bits->nbBits=0;
52    bits->charPtr=0;
53    bits->bitPtr=0;
54    bits->owner=1;
55    bits->overflow=0;
56 }
57
58 void speex_bits_init_buffer(SpeexBits *bits, void *buff, int buf_size)
59 {
60    bits->chars = (char*)buff;
61    bits->buf_size = buf_size;
62
63    bits->chars[0]=0;
64    bits->nbBits=0;
65    bits->charPtr=0;
66    bits->bitPtr=0;
67    bits->owner=0;
68    bits->overflow=0;
69 }
70
71 void speex_bits_destroy(SpeexBits *bits)
72 {
73    if (bits->owner)
74       speex_free(bits->chars);
75    /* Will do something once the allocation is dynamic */
76 }
77
78 void speex_bits_reset(SpeexBits *bits)
79 {
80    bits->chars[0]=0;
81    bits->nbBits=0;
82    bits->charPtr=0;
83    bits->bitPtr=0;
84    bits->overflow=0;
85 }
86
87 void speex_bits_rewind(SpeexBits *bits)
88 {
89    bits->charPtr=0;
90    bits->bitPtr=0;
91    bits->overflow=0;
92 }
93
94 void speex_bits_read_from(SpeexBits *bits, char *chars, int len)
95 {
96    int i;
97    if (len > bits->buf_size)
98    {
99       speex_warning_int("Packet if larger than allocated buffer: ", len);
100       if (bits->owner)
101       {
102          char *tmp = (char*)speex_realloc(bits->chars, len);
103          if (tmp)
104          {
105             bits->buf_size=len;
106             bits->chars=tmp;
107          } else {
108             len=bits->buf_size;
109             speex_warning("Could not resize input buffer: truncating input");
110          }
111       } else {
112          speex_warning("Do not own input buffer: truncating input");
113          len=bits->buf_size;
114       }
115    }
116    for (i=0;i<len;i++)
117       bits->chars[i]=chars[i];
118    bits->nbBits=len<<3;
119    bits->charPtr=0;
120    bits->bitPtr=0;
121    bits->overflow=0;
122 }
123
124 static void speex_bits_flush(SpeexBits *bits)
125 {
126    int i;
127    int nchars = ((bits->nbBits+BITS_PER_CHAR-1)>>LOG2_BITS_PER_CHAR);
128    if (bits->charPtr>0)
129    {
130      for (i=bits->charPtr;i<nchars; i++) 
131        bits->chars[i-bits->charPtr]=bits->chars[i];
132    }
133    bits->nbBits -= bits->charPtr<<LOG2_BITS_PER_CHAR;
134    bits->charPtr=0;
135 }
136
137 void speex_bits_read_whole_bytes(SpeexBits *bits, char *chars, int nbytes)
138 {
139    int i,pos;
140    int nchars = nbytes/BYTES_PER_CHAR;
141
142    if (((bits->nbBits+BITS_PER_CHAR-1)>>LOG2_BITS_PER_CHAR)+nchars > bits->buf_size)
143    {
144       /* Packet is larger than allocated buffer */
145       if (bits->owner)
146       {
147          char *tmp = (char*)speex_realloc(bits->chars, (bits->nbBits>>LOG2_BITS_PER_CHAR)+nchars+1);
148          if (tmp)
149          {
150             bits->buf_size=(bits->nbBits>>LOG2_BITS_PER_CHAR)+nchars+1;
151             bits->chars=tmp;
152          } else {
153             nchars=bits->buf_size-(bits->nbBits>>LOG2_BITS_PER_CHAR)-1;
154             speex_warning("Could not resize input buffer: truncating input");
155          }
156       } else {
157          speex_warning("Do not own input buffer: truncating input");
158          nchars=bits->buf_size;
159       }
160    }
161
162    speex_bits_flush(bits);
163    pos=bits->nbBits>>LOG2_BITS_PER_CHAR;
164    for (i=0;i<nchars;i++)
165       bits->chars[pos+i]=chars[i];
166    bits->nbBits+=nchars<<LOG2_BITS_PER_CHAR;
167 }
168
169 int speex_bits_write(SpeexBits *bits, char *chars, int max_nbytes)
170 {
171    int i;
172    int max_nchars = max_nbytes/BYTES_PER_CHAR;
173    int charPtr, bitPtr, nbBits;
174    
175    /* Insert terminator, but save the data so we can put it back after */
176    bitPtr=bits->bitPtr;
177    charPtr=bits->charPtr;
178    nbBits=bits->nbBits;
179    speex_bits_insert_terminator(bits);
180    bits->bitPtr=bitPtr;
181    bits->charPtr=charPtr;
182    bits->nbBits=nbBits;
183
184    if (max_nchars > ((bits->nbBits+BITS_PER_CHAR-1)>>LOG2_BITS_PER_CHAR))
185       max_nchars = ((bits->nbBits+BITS_PER_CHAR-1)>>LOG2_BITS_PER_CHAR);
186 #if BYTES_PER_CHAR==1
187 #define HTOLS(A) (A)
188 #else
189 #define HTOLS(A) ((((A) >> 8)&0xff)|(((A) & 0xff)<<8))
190 #endif
191    for (i=0;i<max_nchars;i++)
192       chars[i]=HTOLS(bits->chars[i]);
193    return max_nchars*BYTES_PER_CHAR;
194 }
195
196 int speex_bits_write_whole_bytes(SpeexBits *bits, char *chars, int max_nbytes)
197 {
198    int max_nchars = max_nbytes/BYTES_PER_CHAR;
199    int i;
200    if (max_nchars > ((bits->nbBits)>>LOG2_BITS_PER_CHAR))
201       max_nchars = ((bits->nbBits)>>LOG2_BITS_PER_CHAR);
202    for (i=0;i<max_nchars;i++)
203       chars[i]=bits->chars[i];
204    
205    if (bits->bitPtr>0)
206       bits->chars[0]=bits->chars[max_nchars];
207    else
208       bits->chars[0]=0;
209    for (i=1;i<((bits->nbBits)>>LOG2_BITS_PER_CHAR)+1;i++)
210       bits->chars[i]=0;
211    bits->charPtr=0;
212    bits->nbBits &= (BITS_PER_CHAR-1);
213    return max_nchars*BYTES_PER_CHAR;
214 }
215
216 void speex_bits_pack(SpeexBits *bits, int data, int nbBits)
217 {
218    unsigned int d=data;
219
220    if (bits->charPtr+((nbBits+bits->bitPtr)>>LOG2_BITS_PER_CHAR) >= bits->buf_size)
221    {
222       speex_warning("Buffer too small to pack bits");
223       if (bits->owner)
224       {
225         int new_nchars = ((bits->buf_size+5)*3)>>1;
226          char *tmp = (char*)speex_realloc(bits->chars, new_nchars);
227          if (tmp)
228          {
229             speex_memset_bytes(tmp, 0, new_nchars);
230             bits->buf_size=new_nchars;
231             bits->chars=tmp;
232          } else {
233             speex_warning("Could not resize input buffer: not packing");
234             return;
235          }
236       } else {
237          speex_warning("Do not own input buffer: not packing");
238          return;
239       }
240    }
241
242    while(nbBits)
243    {
244       int bit;
245       bit = (d>>(nbBits-1))&1;
246       bits->chars[bits->charPtr] |= bit<<(7-bits->bitPtr);
247       bits->bitPtr++;
248
249       if (bits->bitPtr==8)
250       {
251          bits->bitPtr=0;
252          bits->charPtr++;
253          bits->chars[bits->charPtr] = 0;
254       }
255       bits->nbBits++;
256       nbBits--;
257    }
258 }
259
260 int speex_bits_unpack_signed(SpeexBits *bits, int nbBits)
261 {
262    unsigned int d=speex_bits_unpack_unsigned(bits,nbBits);
263    /* If number is negative */
264    if (d>>(nbBits-1))
265    {
266       d |= (-1)<<nbBits;
267    }
268    return d;
269 }
270
271 unsigned int speex_bits_unpack_unsigned(SpeexBits *bits, int nbBits)
272 {
273    unsigned int d=0;
274    if ((bits->charPtr<<LOG2_BITS_PER_CHAR)+bits->bitPtr+nbBits>bits->nbBits)
275       bits->overflow=1;
276    if (bits->overflow)
277       return 0;
278    while(nbBits)
279    {
280       d<<=1;
281       d |= (bits->chars[bits->charPtr]>>(BITS_PER_CHAR-1 - bits->bitPtr))&1;
282       bits->bitPtr++;
283       if (bits->bitPtr==BITS_PER_CHAR)
284       {
285          bits->bitPtr=0;
286          bits->charPtr++;
287       }
288       nbBits--;
289    }
290    return d;
291 }
292
293 unsigned int speex_bits_peek_unsigned(SpeexBits *bits, int nbBits)
294 {
295    unsigned int d=0;
296    int bitPtr, charPtr;
297    char *chars;
298
299    if ((bits->charPtr<<LOG2_BITS_PER_CHAR)+bits->bitPtr+nbBits>bits->nbBits)
300      bits->overflow=1;
301    if (bits->overflow)
302       return 0;
303
304    bitPtr=bits->bitPtr;
305    charPtr=bits->charPtr;
306    chars = bits->chars;
307    while(nbBits)
308    {
309       d<<=1;
310       d |= (chars[charPtr]>>(BITS_PER_CHAR-1 - bitPtr))&1;
311       bitPtr++;
312       if (bitPtr==BITS_PER_CHAR)
313       {
314          bitPtr=0;
315          charPtr++;
316       }
317       nbBits--;
318    }
319    return d;
320 }
321
322 int speex_bits_peek(SpeexBits *bits)
323 {
324    if ((bits->charPtr<<LOG2_BITS_PER_CHAR)+bits->bitPtr+1>bits->nbBits)
325       bits->overflow=1;
326    if (bits->overflow)
327       return 0;
328    return (bits->chars[bits->charPtr]>>(BITS_PER_CHAR-1 - bits->bitPtr))&1;
329 }
330
331 void speex_bits_advance(SpeexBits *bits, int n)
332 {
333     if (((bits->charPtr<<LOG2_BITS_PER_CHAR)+bits->bitPtr+n>bits->nbBits) || bits->overflow){
334       bits->overflow=1;
335       return;
336     }
337    bits->charPtr += (bits->bitPtr+n) >> LOG2_BITS_PER_CHAR; /* divide by BITS_PER_CHAR */
338    bits->bitPtr = (bits->bitPtr+n) & (BITS_PER_CHAR-1);       /* modulo by BITS_PER_CHAR */
339 }
340
341 int speex_bits_remaining(SpeexBits *bits)
342 {
343    if (bits->overflow)
344       return -1;
345    else
346       return bits->nbBits-((bits->charPtr<<LOG2_BITS_PER_CHAR)+bits->bitPtr);
347 }
348
349 int speex_bits_nbytes(SpeexBits *bits)
350 {
351    return ((bits->nbBits+BITS_PER_CHAR-1)>>LOG2_BITS_PER_CHAR);
352 }
353
354 void speex_bits_insert_terminator(SpeexBits *bits)
355 {
356    if (bits->bitPtr<BITS_PER_CHAR-1)
357       speex_bits_pack(bits, 0, 1);
358    while (bits->bitPtr<BITS_PER_CHAR-1)
359       speex_bits_pack(bits, 1, 1);
360 }