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
49extern 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
59static long ADJUST_SQRT2[2]={8192,5792};
60static 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_
73static inline ogg_int32_t vorbis_fromdBlook_i(long a){
74  if(a>0) return 0x7fffffff;
75  if(a<(-140<<12)) return 0;
76  return FLOOR_fromdB_LOOKUP[((a+140)*467)>>20]<<9;
77}
78#else
79static inline ogg_int32_t vorbis_fromdBlook_i(long a){
80  if(a>0) return 0x7fffffff;
81  if(a<(-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 */
88static 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 */
97static 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
105static 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 */
116static 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
129static 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
136static 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
143static const unsigned char MLOOP_3[8]={0,1,2,2,3,3,3,3};
144
145void 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
361void floor0_free_info(vorbis_info_floor *i){
362  vorbis_info_floor0 *info=(vorbis_info_floor0 *)i;
363  if(info)_ogg_free(info);
364}
365
366vorbis_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
395int floor0_memosize(vorbis_info_floor *i){
396  vorbis_info_floor0 *info=(vorbis_info_floor0 *)i;
397  return info->order+1;
398}
399
400ogg_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
431int 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