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