1 /************************************************************************
2  * Copyright (C) 2002-2009, Xiph.org Foundation
3  * Copyright (C) 2010, Robin Watts for Pinknoise Productions Ltd
4  * All rights reserved.
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  *     * Redistributions in binary form must reproduce the above
13  * copyright notice, this list of conditions and the following disclaimer
14  * in the documentation and/or other materials provided with the
15  * distribution.
16  *     * Neither the names of the Xiph.org Foundation nor Pinknoise
17  * Productions Ltd nor the names of its contributors may be used to
18  * endorse or promote products derived from this software without
19  * 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 COPYRIGHT
25  * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
26  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
27  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
28  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
29  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
30  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
31  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
32  ************************************************************************
33 
34  function: floor backend 0 implementation
35 
36  ************************************************************************/
37 
38 #include <stdlib.h>
39 #include <string.h>
40 #include <math.h>
41 #include "ogg.h"
42 #include "ivorbiscodec.h"
43 #include "codec_internal.h"
44 #include "codebook.h"
45 #include "misc.h"
46 #include "os.h"
47 
48 #define LSP_FRACBITS 14
49 extern const ogg_int32_t FLOOR_fromdB_LOOKUP[];
50 
51 /*************** LSP decode ********************/
52 
53 #include "lsp_lookup.h"
54 
55 /* interpolated 1./sqrt(p) where .5 <= a < 1. (.100000... to .111111...) in
56    16.16 format
57    returns in m.8 format */
58 
59 static long ADJUST_SQRT2[2]={8192,5792};
vorbis_invsqlook_i(long a,long e)60 static inline ogg_int32_t vorbis_invsqlook_i(long a,long e){
61   long i=(a&0x7fff)>>(INVSQ_LOOKUP_I_SHIFT-1);
62   long d=a&INVSQ_LOOKUP_I_MASK;                              /*  0.10 */
63   long val=INVSQ_LOOKUP_I[i]-                                /*  1.16 */
64     ((INVSQ_LOOKUP_IDel[i]*d)>>INVSQ_LOOKUP_I_SHIFT);        /* result 1.16 */
65   val*=ADJUST_SQRT2[e&1];
66   e=(e>>1)+21;
67   return(val>>e);
68 }
69 
70 /* interpolated lookup based fromdB function, domain -140dB to 0dB only */
71 /* a is in n.12 format */
72 #ifdef _LOW_ACCURACY_
vorbis_fromdBlook_i(long a)73 static inline ogg_int32_t vorbis_fromdBlook_i(long a){
74   if(a>0) return 0x7fffffff;
75   if(a<(int)(((unsigned)-140)<<12)) return 0;
76   return FLOOR_fromdB_LOOKUP[((a+140)*467)>>20]<<9;
77 }
78 #else
vorbis_fromdBlook_i(long a)79 static inline ogg_int32_t vorbis_fromdBlook_i(long a){
80   if(a>0) return 0x7fffffff;
81   if(a<(int)(((unsigned)-140)<<12)) return 0;
82   return FLOOR_fromdB_LOOKUP[((a+(140<<12))*467)>>20];
83 }
84 #endif
85 
86 /* interpolated lookup based cos function, domain 0 to PI only */
87 /* a is in 0.16 format, where 0==0, 2^^16-1==PI, return 0.14 */
vorbis_coslook_i(long a)88 static inline ogg_int32_t vorbis_coslook_i(long a){
89   int i=a>>COS_LOOKUP_I_SHIFT;
90   int d=a&COS_LOOKUP_I_MASK;
91   return COS_LOOKUP_I[i]- ((d*(COS_LOOKUP_I[i]-COS_LOOKUP_I[i+1]))>>
92 			   COS_LOOKUP_I_SHIFT);
93 }
94 
95 /* interpolated half-wave lookup based cos function */
96 /* a is in 0.16 format, where 0==0, 2^^16==PI, return .LSP_FRACBITS */
vorbis_coslook2_i(long a)97 static inline ogg_int32_t vorbis_coslook2_i(long a){
98   int i=a>>COS_LOOKUP_I_SHIFT;
99   int d=a&COS_LOOKUP_I_MASK;
100   return ((COS_LOOKUP_I[i]<<COS_LOOKUP_I_SHIFT)-
101 	  d*(COS_LOOKUP_I[i]-COS_LOOKUP_I[i+1]))>>
102     (COS_LOOKUP_I_SHIFT-LSP_FRACBITS+14);
103 }
104 
105 static const ogg_uint16_t barklook[54]={
106   0,51,102,154,            206,258,311,365,
107   420,477,535,594,         656,719,785,854,
108   926,1002,1082,1166,      1256,1352,1454,1564,
109   1683,1812,1953,2107,     2276,2463,2670,2900,
110   3155,3440,3756,4106,     4493,4919,5387,5901,
111   6466,7094,7798,8599,     9528,10623,11935,13524,
112   15453,17775,20517,23667, 27183,31004
113 };
114 
115 /* used in init only; interpolate the long way */
toBARK(int n)116 static inline ogg_int32_t toBARK(int n){
117   int i;
118   for(i=0;i<54;i++)
119     if(n>=barklook[i] && n<barklook[i+1])break;
120 
121   if(i==54){
122     return 54<<14;
123   }else{
124     return (i<<14)+(((n-barklook[i])*
125 		     ((1UL<<31)/(barklook[i+1]-barklook[i])))>>17);
126   }
127 }
128 
129 static const unsigned char MLOOP_1[64]={
130    0,10,11,11, 12,12,12,12, 13,13,13,13, 13,13,13,13,
131   14,14,14,14, 14,14,14,14, 14,14,14,14, 14,14,14,14,
132   15,15,15,15, 15,15,15,15, 15,15,15,15, 15,15,15,15,
133   15,15,15,15, 15,15,15,15, 15,15,15,15, 15,15,15,15,
134 };
135 
136 static const unsigned char MLOOP_2[64]={
137   0,4,5,5, 6,6,6,6, 7,7,7,7, 7,7,7,7,
138   8,8,8,8, 8,8,8,8, 8,8,8,8, 8,8,8,8,
139   9,9,9,9, 9,9,9,9, 9,9,9,9, 9,9,9,9,
140   9,9,9,9, 9,9,9,9, 9,9,9,9, 9,9,9,9,
141 };
142 
143 static const unsigned char MLOOP_3[8]={0,1,2,2,3,3,3,3};
144 
vorbis_lsp_to_curve(ogg_int32_t * curve,int n,int ln,ogg_int32_t * lsp,int m,ogg_int32_t amp,ogg_int32_t ampoffset,ogg_int32_t nyq)145 void vorbis_lsp_to_curve(ogg_int32_t *curve,int n,int ln,
146 			 ogg_int32_t *lsp,int m,
147 			 ogg_int32_t amp,
148 			 ogg_int32_t ampoffset,
149 			 ogg_int32_t nyq){
150 
151   /* 0 <= m < 256 */
152 
153   /* set up for using all int later */
154   int i;
155   int ampoffseti=ampoffset*4096;
156   int ampi=amp;
157   ogg_int32_t *ilsp=(ogg_int32_t *)alloca(m*sizeof(*ilsp));
158 
159   ogg_uint32_t inyq= (1UL<<31) / toBARK(nyq);
160   ogg_uint32_t imap= (1UL<<31) / ln;
161   ogg_uint32_t tBnyq1 = toBARK(nyq)<<1;
162 
163   /* Besenham for frequency scale to avoid a division */
164   int f=0;
165   int fdx=n;
166   int fbase=nyq/fdx;
167   int ferr=0;
168   int fdy=nyq-fbase*fdx;
169   int map=0;
170 
171 #ifdef _LOW_ACCURACY_
172   ogg_uint32_t nextbark=((tBnyq1<<11)/ln)>>12;
173 #else
174   ogg_uint32_t nextbark=MULT31(imap>>1,tBnyq1);
175 #endif
176   int nextf=barklook[nextbark>>14]+(((nextbark&0x3fff)*
177 	    (barklook[(nextbark>>14)+1]-barklook[nextbark>>14]))>>14);
178 
179   /* lsp is in 8.24, range 0 to PI; coslook wants it in .16 0 to 1*/
180   for(i=0;i<m;i++){
181 #ifndef _LOW_ACCURACY_
182     ogg_int32_t val=MULT32(lsp[i],0x517cc2);
183 #else
184     ogg_int32_t val=((lsp[i]>>10)*0x517d)>>14;
185 #endif
186 
187     /* safeguard against a malicious stream */
188     if(val<0 || (val>>COS_LOOKUP_I_SHIFT)>=COS_LOOKUP_I_SZ){
189       memset(curve,0,sizeof(*curve)*n);
190       return;
191     }
192 
193     ilsp[i]=vorbis_coslook_i(val);
194   }
195 
196   i=0;
197   while(i<n){
198     int j;
199     ogg_uint32_t pi=46341; /* 2**-.5 in 0.16 */
200     ogg_uint32_t qi=46341;
201     ogg_int32_t qexp=0,shift;
202     ogg_int32_t wi;
203 
204     wi=vorbis_coslook2_i((map*imap)>>15);
205 
206 
207 #ifdef _V_LSP_MATH_ASM
208     lsp_loop_asm(&qi,&pi,&qexp,ilsp,wi,m);
209 
210     pi=((pi*pi)>>16);
211     qi=((qi*qi)>>16);
212 
213     if(m&1){
214       qexp= qexp*2-28*((m+1)>>1)+m;
215       pi*=(1<<14)-((wi*wi)>>14);
216       qi+=pi>>14;
217     }else{
218       qexp= qexp*2-13*m;
219 
220       pi*=(1<<14)-wi;
221       qi*=(1<<14)+wi;
222 
223       qi=(qi+pi)>>14;
224     }
225 
226     if(qi&0xffff0000){ /* checks for 1.xxxxxxxxxxxxxxxx */
227       qi>>=1; qexp++;
228     }else
229       lsp_norm_asm(&qi,&qexp);
230 
231 #else
232 
233     qi*=labs(ilsp[0]-wi);
234     pi*=labs(ilsp[1]-wi);
235 
236     for(j=3;j<m;j+=2){
237       if(!(shift=MLOOP_1[(pi|qi)>>25]))
238       	if(!(shift=MLOOP_2[(pi|qi)>>19]))
239       	  shift=MLOOP_3[(pi|qi)>>16];
240 
241       qi=(qi>>shift)*labs(ilsp[j-1]-wi);
242       pi=(pi>>shift)*labs(ilsp[j]-wi);
243       qexp+=shift;
244     }
245     if(!(shift=MLOOP_1[(pi|qi)>>25]))
246       if(!(shift=MLOOP_2[(pi|qi)>>19]))
247 	shift=MLOOP_3[(pi|qi)>>16];
248 
249     /* pi,qi normalized collectively, both tracked using qexp */
250 
251     if(m&1){
252       /* odd order filter; slightly assymetric */
253       /* the last coefficient */
254       qi=(qi>>shift)*labs(ilsp[j-1]-wi);
255       pi=(pi>>shift)<<14;
256       qexp+=shift;
257 
258       if(!(shift=MLOOP_1[(pi|qi)>>25]))
259 	if(!(shift=MLOOP_2[(pi|qi)>>19]))
260 	  shift=MLOOP_3[(pi|qi)>>16];
261 
262       pi>>=shift;
263       qi>>=shift;
264       qexp+=shift-14*((m+1)>>1);
265 
266       pi=((pi*pi)>>16);
267       qi=((qi*qi)>>16);
268       qexp=qexp*2+m;
269 
270       pi*=(1<<14)-((wi*wi)>>14);
271       qi+=pi>>14;
272 
273     }else{
274       /* even order filter; still symmetric */
275 
276       /* p*=p(1-w), q*=q(1+w), let normalization drift because it isn't
277 	 worth tracking step by step */
278 
279       pi>>=shift;
280       qi>>=shift;
281       qexp+=shift-7*m;
282 
283       pi=((pi*pi)>>16);
284       qi=((qi*qi)>>16);
285       qexp=qexp*2+m;
286 
287       pi*=(1<<14)-wi;
288       qi*=(1<<14)+wi;
289       qi=(qi+pi)>>14;
290 
291     }
292 
293 
294     /* we've let the normalization drift because it wasn't important;
295        however, for the lookup, things must be normalized again.  We
296        need at most one right shift or a number of left shifts */
297 
298     if(qi&0xffff0000){ /* checks for 1.xxxxxxxxxxxxxxxx */
299       qi>>=1; qexp++;
300     }else
301       while(qi && !(qi&0x8000)){ /* checks for 0.0xxxxxxxxxxxxxxx or less*/
302 	qi<<=1; qexp--;
303       }
304 
305 #endif
306 
307     amp=vorbis_fromdBlook_i(ampi*                     /*  n.4         */
308 			    vorbis_invsqlook_i(qi,qexp)-
309 			                              /*  m.8, m+n<=8 */
310 			    ampoffseti);              /*  8.12[0]     */
311 
312 #ifdef _LOW_ACCURACY_
313     amp>>=9;
314 #endif
315     curve[i]= MULT31_SHIFT15(curve[i],amp);
316 
317     while(++i<n){
318 
319       /* line plot to get new f */
320       ferr+=fdy;
321       if(ferr>=fdx){
322 	ferr-=fdx;
323 	f++;
324       }
325       f+=fbase;
326 
327       if(f>=nextf)break;
328 
329       curve[i]= MULT31_SHIFT15(curve[i],amp);
330     }
331 
332     while(1){
333       map++;
334 
335       if(map+1<ln){
336 
337 #ifdef _LOW_ACCURACY_
338 	nextbark=((tBnyq1<<11)/ln*(map+1))>>12;
339 #else
340 	nextbark=MULT31((map+1)*(imap>>1),tBnyq1);
341 #endif
342 	nextf=barklook[nextbark>>14]+
343 	  (((nextbark&0x3fff)*
344 	    (barklook[(nextbark>>14)+1]-barklook[nextbark>>14]))>>14);
345 	if(f<=nextf)break;
346 
347       }else{
348 	nextf=9999999;
349 	break;
350       }
351     }
352     if(map>=ln){
353       map=ln-1; /* guard against the approximation */
354       nextf=9999999;
355     }
356   }
357 }
358 
359 /*************** vorbis decode glue ************/
360 
floor0_free_info(vorbis_info_floor * i)361 void floor0_free_info(vorbis_info_floor *i){
362   vorbis_info_floor0 *info=(vorbis_info_floor0 *)i;
363   if(info)_ogg_free(info);
364 }
365 
floor0_info_unpack(vorbis_info * vi,oggpack_buffer * opb)366 vorbis_info_floor *floor0_info_unpack (vorbis_info *vi,oggpack_buffer *opb){
367   codec_setup_info     *ci=(codec_setup_info *)vi->codec_setup;
368   int j;
369 
370   vorbis_info_floor0 *info=(vorbis_info_floor0 *)_ogg_malloc(sizeof(*info));
371   info->order=oggpack_read(opb,8);
372   info->rate=oggpack_read(opb,16);
373   info->barkmap=oggpack_read(opb,16);
374   info->ampbits=oggpack_read(opb,6);
375   info->ampdB=oggpack_read(opb,8);
376   info->numbooks=oggpack_read(opb,4)+1;
377 
378   if(info->order<1)goto err_out;
379   if(info->rate<1)goto err_out;
380   if(info->barkmap<1)goto err_out;
381 
382   for(j=0;j<info->numbooks;j++){
383     info->books[j]=(char)oggpack_read(opb,8);
384     if(info->books[j]>=ci->books)goto err_out;
385   }
386 
387   if(oggpack_eop(opb))goto err_out;
388   return(info);
389 
390  err_out:
391   floor0_free_info(info);
392   return(NULL);
393 }
394 
floor0_memosize(vorbis_info_floor * i)395 int floor0_memosize(vorbis_info_floor *i){
396   vorbis_info_floor0 *info=(vorbis_info_floor0 *)i;
397   return info->order+1;
398 }
399 
floor0_inverse1(vorbis_dsp_state * vd,vorbis_info_floor * i,ogg_int32_t * lsp)400 ogg_int32_t *floor0_inverse1(vorbis_dsp_state *vd,vorbis_info_floor *i,
401 			     ogg_int32_t *lsp){
402   vorbis_info_floor0 *info=(vorbis_info_floor0 *)i;
403   int j,k;
404 
405   int ampraw=oggpack_read(&vd->opb,info->ampbits);
406   if(ampraw>0){ /* also handles the -1 out of data case */
407     long maxval=(1<<info->ampbits)-1;
408     int amp=((ampraw*info->ampdB)<<4)/maxval;
409     int booknum=oggpack_read(&vd->opb,_ilog(info->numbooks));
410 
411     if(booknum!=-1 && booknum<info->numbooks){ /* be paranoid */
412       codec_setup_info  *ci=(codec_setup_info *)vd->vi->codec_setup;
413       codebook *b=ci->book_param+info->books[booknum];
414       ogg_int32_t last=0;
415 
416       for(j=0;j<info->order;j+=b->dim)
417 	if(vorbis_book_decodev_set(b,lsp+j,&vd->opb,b->dim,-24)==-1)goto eop;
418       for(j=0;j<info->order;){
419 	for(k=0;k<b->dim;k++,j++)lsp[j]+=last;
420 	last=lsp[j-1];
421       }
422 
423       lsp[info->order]=amp;
424       return(lsp);
425     }
426   }
427  eop:
428   return(NULL);
429 }
430 
floor0_inverse2(vorbis_dsp_state * vd,vorbis_info_floor * i,ogg_int32_t * lsp,ogg_int32_t * out)431 int floor0_inverse2(vorbis_dsp_state *vd,vorbis_info_floor *i,
432 			   ogg_int32_t *lsp,ogg_int32_t *out){
433   vorbis_info_floor0 *info=(vorbis_info_floor0 *)i;
434   codec_setup_info  *ci=(codec_setup_info *)vd->vi->codec_setup;
435 
436   if(lsp){
437     ogg_int32_t amp=lsp[info->order];
438 
439     /* take the coefficients back to a spectral envelope curve */
440     vorbis_lsp_to_curve(out,ci->blocksizes[vd->W]/2,info->barkmap,
441 			lsp,info->order,amp,info->ampdB,
442 			info->rate>>1);
443     return(1);
444   }
445   memset(out,0,sizeof(*out)*ci->blocksizes[vd->W]/2);
446   return(0);
447 }
448 
449