oops...
[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 #include "speex_bits.h"
36 #include "misc.h"
37
38 void speex_bits_init(SpeexBits *bits)
39 {
40    int i;
41    bits->bytes = (char*)speex_alloc(MAX_BYTES_PER_FRAME);
42    bits->buf_size = MAX_BYTES_PER_FRAME;
43
44    for (i=0;i<bits->buf_size;i++)
45       bits->bytes[i]=0;
46    bits->nbBits=0;
47    bits->bytePtr=0;
48    bits->bitPtr=0;
49    bits->owner=1;
50    bits->overflow=0;
51 }
52
53 void speex_bits_init_buffer(SpeexBits *bits, void *buff, int buf_size)
54 {
55    int i;
56    bits->bytes = (char*)buff;
57    bits->buf_size = buf_size;
58
59    for (i=0;i<buf_size;i++)
60       bits->bytes[i]=0;
61    bits->nbBits=0;
62    bits->bytePtr=0;
63    bits->bitPtr=0;
64    bits->owner=0;
65    bits->overflow=0;
66 }
67
68 void speex_bits_destroy(SpeexBits *bits)
69 {
70    if (bits->owner)
71       speex_free(bits->bytes);
72    /* Will do something once the allocation is dynamic */
73 }
74
75 void speex_bits_reset(SpeexBits *bits)
76 {
77    int i;
78    for (i=0;i<bits->buf_size;i++)
79       bits->bytes[i]=0;
80    bits->nbBits=0;
81    bits->bytePtr=0;
82    bits->bitPtr=0;
83    bits->overflow=0;
84 }
85
86 void speex_bits_rewind(SpeexBits *bits)
87 {
88    bits->bytePtr=0;
89    bits->bitPtr=0;
90    bits->overflow=0;
91 }
92
93 void speex_bits_read_from(SpeexBits *bits, char *bytes, int len)
94 {
95    int i;
96    if (len > bits->buf_size)
97    {
98       speex_warning_int("Packet if larger than allocated buffer: ", len);
99       if (bits->owner)
100       {
101          char *tmp = (char*)speex_realloc(bits->bytes, len);
102          if (tmp)
103          {
104             bits->buf_size=len;
105             bits->bytes=tmp;
106          } else {
107             len=bits->buf_size;
108             speex_warning("Could not resize input buffer: truncating input");
109          }
110       } else {
111          speex_warning("Do not own input buffer: truncating input");
112          len=bits->buf_size;
113       }
114    }
115    for (i=0;i<len;i++)
116       bits->bytes[i]=bytes[i];
117    bits->nbBits=len<<3;
118    bits->bytePtr=0;
119    bits->bitPtr=0;
120    bits->overflow=0;
121 }
122
123 static void speex_bits_flush(SpeexBits *bits)
124 {
125    int i;
126    if (bits->bytePtr>0)
127    {
128       for (i=bits->bytePtr;i<((bits->nbBits+7)>>3);i++)
129          bits->bytes[i-bits->bytePtr]=bits->bytes[i];
130    }
131    bits->nbBits -= bits->bytePtr<<3;
132    bits->bytePtr=0;
133 }
134
135 void speex_bits_read_whole_bytes(SpeexBits *bits, char *bytes, int len)
136 {
137    int i,pos;
138
139    if ((bits->nbBits>>3)+len+1 > bits->buf_size)
140    {
141       speex_warning_int("Packet if larger than allocated buffer: ", len);
142       if (bits->owner)
143       {
144          char *tmp = (char*)speex_realloc(bits->bytes, (bits->nbBits>>3)+len+1);
145          if (tmp)
146          {
147             bits->buf_size=(bits->nbBits>>3)+len+1;
148             bits->bytes=tmp;
149          } else {
150             len=bits->buf_size-(bits->nbBits>>3)-1;
151             speex_warning("Could not resize input buffer: truncating input");
152          }
153       } else {
154          speex_warning("Do not own input buffer: truncating input");
155          len=bits->buf_size;
156       }
157    }
158
159    speex_bits_flush(bits);
160    pos=bits->nbBits>>3;
161    for (i=0;i<len;i++)
162       bits->bytes[pos+i]=bytes[i];
163    bits->nbBits+=len<<3;
164 }
165
166 int speex_bits_write(SpeexBits *bits, char *bytes, int max_len)
167 {
168    int i;
169    int bytePtr, bitPtr, nbBits;
170    
171    /* Insert terminator, but save the data so we can put it back after */
172    bitPtr=bits->bitPtr;
173    bytePtr=bits->bytePtr;
174    nbBits=bits->nbBits;
175    speex_bits_insert_terminator(bits);
176    bits->bitPtr=bitPtr;
177    bits->bytePtr=bytePtr;
178    bits->nbBits=nbBits;
179
180    if (max_len > ((bits->nbBits+7)>>3))
181       max_len = ((bits->nbBits+7)>>3);
182    for (i=0;i<max_len;i++)
183       bytes[i]=bits->bytes[i];
184    return max_len;
185 }
186
187 int speex_bits_write_whole_bytes(SpeexBits *bits, char *bytes, int max_len)
188 {
189    int i;
190    if (max_len > ((bits->nbBits)>>3))
191       max_len = ((bits->nbBits)>>3);
192    for (i=0;i<max_len;i++)
193       bytes[i]=bits->bytes[i];
194    
195    if (bits->bitPtr>0)
196       bits->bytes[0]=bits->bytes[max_len];
197    else
198       bits->bytes[0]=0;
199    for (i=1;i<((bits->nbBits)>>3)+1;i++)
200       bits->bytes[i]=0;
201    bits->bytePtr=0;
202    bits->nbBits &= 7;
203    return max_len;
204 }
205
206
207 void speex_bits_pack(SpeexBits *bits, int data, int nbBits)
208 {
209    int i;
210    unsigned int d=data;
211
212    if (bits->bytePtr+((nbBits+bits->bitPtr)>>3) >= bits->buf_size)
213    {
214       speex_warning("Buffer too small to pack bits");
215       if (bits->owner)
216       {
217          char *tmp = (char*)speex_realloc(bits->bytes, ((bits->buf_size+5)*3)>>1);
218          if (tmp)
219          {
220             for (i=bits->buf_size;i<(((bits->buf_size+5)*3)>>1);i++)
221                tmp[i]=0;
222             bits->buf_size=((bits->buf_size+5)*3)>>1;
223             bits->bytes=tmp;
224          } else {
225             speex_warning("Could not resize input buffer: not packing");
226             return;
227          }
228       } else {
229          speex_warning("Do not own input buffer: not packing");
230          return;
231       }
232    }
233
234    while(nbBits)
235    {
236       int bit;
237       bit = (d>>(nbBits-1))&1;
238       bits->bytes[bits->bytePtr] |= bit<<(7-bits->bitPtr);
239       bits->bitPtr++;
240
241       if (bits->bitPtr==8)
242       {
243          bits->bitPtr=0;
244          bits->bytePtr++;
245       }
246       bits->nbBits++;
247       nbBits--;
248    }
249 }
250
251 int speex_bits_unpack_signed(SpeexBits *bits, int nbBits)
252 {
253    unsigned int d=speex_bits_unpack_unsigned(bits,nbBits);
254    /* If number is negative */
255    if (d>>(nbBits-1))
256    {
257       d |= (-1)<<nbBits;
258    }
259    return d;
260 }
261
262 unsigned int speex_bits_unpack_unsigned(SpeexBits *bits, int nbBits)
263 {
264    unsigned int d=0;
265    if ((bits->bytePtr<<3)+bits->bitPtr+nbBits>bits->nbBits)
266       bits->overflow=1;
267    if (bits->overflow)
268       return 0;
269    while(nbBits)
270    {
271       d<<=1;
272       d |= (bits->bytes[bits->bytePtr]>>(7-bits->bitPtr))&1;
273       bits->bitPtr++;
274       if (bits->bitPtr==8)
275       {
276          bits->bitPtr=0;
277          bits->bytePtr++;
278       }
279       nbBits--;
280    }
281    return d;
282 }
283
284 unsigned int speex_bits_peek_unsigned(SpeexBits *bits, int nbBits)
285 {
286    unsigned int d=0;
287    int bitPtr, bytePtr;
288    char *bytes;
289
290    if ((bits->bytePtr<<3)+bits->bitPtr+nbBits>bits->nbBits)
291      bits->overflow=1;
292    if (bits->overflow)
293       return 0;
294
295    bitPtr=bits->bitPtr;
296    bytePtr=bits->bytePtr;
297    bytes = bits->bytes;
298    while(nbBits)
299    {
300       d<<=1;
301       d |= (bytes[bytePtr]>>(7-bitPtr))&1;
302       bitPtr++;
303       if (bitPtr==8)
304       {
305          bitPtr=0;
306          bytePtr++;
307       }
308       nbBits--;
309    }
310    return d;
311 }
312
313 int speex_bits_peek(SpeexBits *bits)
314 {
315    if ((bits->bytePtr<<3)+bits->bitPtr+1>bits->nbBits)
316       bits->overflow=1;
317    if (bits->overflow)
318       return 0;
319    return (bits->bytes[bits->bytePtr]>>(7-bits->bitPtr))&1;
320 }
321
322 void speex_bits_advance(SpeexBits *bits, int n)
323 {
324    int nbytes, nbits;
325
326    if ((bits->bytePtr<<3)+bits->bitPtr+n>bits->nbBits)
327       bits->overflow=1;
328    if (bits->overflow)
329       return;
330
331    nbytes = n >> 3;
332    nbits = n & 7;
333    
334    bits->bytePtr += nbytes;
335    bits->bitPtr += nbits;
336    
337    if (bits->bitPtr>7)
338    {
339       bits->bitPtr-=8;
340       bits->bytePtr++;
341    }
342 }
343
344 int speex_bits_remaining(SpeexBits *bits)
345 {
346    if (bits->overflow)
347       return -1;
348    else
349       return bits->nbBits-((bits->bytePtr<<3)+bits->bitPtr);
350 }
351
352 int speex_bits_nbytes(SpeexBits *bits)
353 {
354    return ((bits->nbBits+7)>>3);
355 }
356
357 void speex_bits_insert_terminator(SpeexBits *bits)
358 {
359    if (bits->bitPtr<7)
360       speex_bits_pack(bits, 0, 1);
361    while (bits->bitPtr<7)
362       speex_bits_pack(bits, 1, 1);
363 }