1 /* Copyright (C) 2002 Jean-Marc Valin
2    File: quant_lsp.c
3    LSP vector quantization
4 
5    Redistribution and use in source and binary forms, with or without
6    modification, are permitted provided that the following conditions
7    are met:
8 
9    - Redistributions of source code must retain the above copyright
10    notice, this list of conditions and the following disclaimer.
11 
12    - Redistributions in binary form must reproduce the above copyright
13    notice, this list of conditions and the following disclaimer in the
14    documentation and/or other materials provided with the distribution.
15 
16    - Neither the name of the Xiph.org Foundation nor the names of its
17    contributors may be used to endorse or promote products derived from
18    this software without specific prior written permission.
19 
20    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
23    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
24    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
25    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
26    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
27    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
28    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
29    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
30    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31 */
32 
33 #ifdef HAVE_CONFIG_H
34 #include "config.h"
35 #endif
36 
37 #include "quant_lsp.h"
38 #include "os_support.h"
39 #include <math.h>
40 #ifndef M_PI
41 #define M_PI 3.14159265358979323846
42 #endif
43 
44 #include "arch.h"
45 
46 #ifdef BFIN_ASM
47 #include "quant_lsp_bfin.h"
48 #endif
49 
50 #ifdef FIXED_POINT
51 
52 #define LSP_LINEAR(i) (SHL16(i+1,11))
53 #define LSP_LINEAR_HIGH(i) (ADD16(MULT16_16_16(i,2560),6144))
54 #define LSP_DIV_256(x) (SHL16((spx_word16_t)x, 5))
55 #define LSP_DIV_512(x) (SHL16((spx_word16_t)x, 4))
56 #define LSP_DIV_1024(x) (SHL16((spx_word16_t)x, 3))
57 #define LSP_PI 25736
58 
59 #else
60 
61 #define LSP_LINEAR(i) (.25*(i)+.25)
62 #define LSP_LINEAR_HIGH(i) (.3125*(i)+.75)
63 #define LSP_SCALE 256.
64 #define LSP_DIV_256(x) (0.0039062*(x))
65 #define LSP_DIV_512(x) (0.0019531*(x))
66 #define LSP_DIV_1024(x) (0.00097656*(x))
67 #define LSP_PI M_PI
68 
69 #endif
70 
compute_quant_weights(spx_lsp_t * qlsp,spx_word16_t * quant_weight,int order)71 static void compute_quant_weights(spx_lsp_t *qlsp, spx_word16_t *quant_weight, int order)
72 {
73    int i;
74    spx_word16_t tmp1, tmp2;
75    for (i=0;i<order;i++)
76    {
77       if (i==0)
78          tmp1 = qlsp[i];
79       else
80          tmp1 = qlsp[i]-qlsp[i-1];
81       if (i==order-1)
82          tmp2 = LSP_PI-qlsp[i];
83       else
84          tmp2 = qlsp[i+1]-qlsp[i];
85       if (tmp2<tmp1)
86          tmp1 = tmp2;
87 #ifdef FIXED_POINT
88       quant_weight[i] = DIV32_16(81920,ADD16(300,tmp1));
89 #else
90       quant_weight[i] = 10/(.04+tmp1);
91 #endif
92    }
93 
94 }
95 
96 /* Note: x is modified*/
97 #ifndef OVERRIDE_LSP_QUANT
lsp_quant(spx_word16_t * x,const signed char * cdbk,int nbVec,int nbDim)98 static int lsp_quant(spx_word16_t *x, const signed char *cdbk, int nbVec, int nbDim)
99 {
100    int i,j;
101    spx_word32_t dist;
102    spx_word16_t tmp;
103    spx_word32_t best_dist=VERY_LARGE32;
104    int best_id=0;
105    const signed char *ptr=cdbk;
106    for (i=0;i<nbVec;i++)
107    {
108       dist=0;
109       for (j=0;j<nbDim;j++)
110       {
111          tmp=SUB16(x[j],SHL16((spx_word16_t)*ptr++,5));
112          dist=MAC16_16(dist,tmp,tmp);
113       }
114       if (dist<best_dist)
115       {
116          best_dist=dist;
117          best_id=i;
118       }
119    }
120 
121    for (j=0;j<nbDim;j++)
122       x[j] = SUB16(x[j],SHL16((spx_word16_t)cdbk[best_id*nbDim+j],5));
123 
124    return best_id;
125 }
126 #endif
127 
128 /* Note: x is modified*/
129 #ifndef OVERRIDE_LSP_WEIGHT_QUANT
lsp_weight_quant(spx_word16_t * x,spx_word16_t * weight,const signed char * cdbk,int nbVec,int nbDim)130 static int lsp_weight_quant(spx_word16_t *x, spx_word16_t *weight, const signed char *cdbk, int nbVec, int nbDim)
131 {
132    int i,j;
133    spx_word32_t dist;
134    spx_word16_t tmp;
135    spx_word32_t best_dist=VERY_LARGE32;
136    int best_id=0;
137    const signed char *ptr=cdbk;
138    for (i=0;i<nbVec;i++)
139    {
140       dist=0;
141       for (j=0;j<nbDim;j++)
142       {
143          tmp=SUB16(x[j],SHL16((spx_word16_t)*ptr++,5));
144          dist=MAC16_32_Q15(dist,weight[j],MULT16_16(tmp,tmp));
145       }
146       if (dist<best_dist)
147       {
148          best_dist=dist;
149          best_id=i;
150       }
151    }
152 
153    for (j=0;j<nbDim;j++)
154       x[j] = SUB16(x[j],SHL16((spx_word16_t)cdbk[best_id*nbDim+j],5));
155    return best_id;
156 }
157 #endif
158 
lsp_quant_nb(spx_lsp_t * lsp,spx_lsp_t * qlsp,int order,SpeexBits * bits)159 void lsp_quant_nb(spx_lsp_t *lsp, spx_lsp_t *qlsp, int order, SpeexBits *bits)
160 {
161    int i;
162    int id;
163    spx_word16_t quant_weight[10];
164 
165    for (i=0;i<order;i++)
166       qlsp[i]=lsp[i];
167 
168    compute_quant_weights(qlsp, quant_weight, order);
169 
170    for (i=0;i<order;i++)
171       qlsp[i]=SUB16(qlsp[i],LSP_LINEAR(i));
172 
173 #ifndef FIXED_POINT
174    for (i=0;i<order;i++)
175       qlsp[i] = LSP_SCALE*qlsp[i];
176 #endif
177    id = lsp_quant(qlsp, cdbk_nb, NB_CDBK_SIZE, order);
178    speex_bits_pack(bits, id, 6);
179 
180    for (i=0;i<order;i++)
181       qlsp[i]*=2;
182 
183    id = lsp_weight_quant(qlsp, quant_weight, cdbk_nb_low1, NB_CDBK_SIZE_LOW1, 5);
184    speex_bits_pack(bits, id, 6);
185 
186    for (i=0;i<5;i++)
187       qlsp[i]*=2;
188 
189    id = lsp_weight_quant(qlsp, quant_weight, cdbk_nb_low2, NB_CDBK_SIZE_LOW2, 5);
190    speex_bits_pack(bits, id, 6);
191 
192    id = lsp_weight_quant(qlsp+5, quant_weight+5, cdbk_nb_high1, NB_CDBK_SIZE_HIGH1, 5);
193    speex_bits_pack(bits, id, 6);
194 
195    for (i=5;i<10;i++)
196       qlsp[i]*=2;
197 
198    id = lsp_weight_quant(qlsp+5, quant_weight+5, cdbk_nb_high2, NB_CDBK_SIZE_HIGH2, 5);
199    speex_bits_pack(bits, id, 6);
200 
201 #ifdef FIXED_POINT
202    for (i=0;i<order;i++)
203       qlsp[i]=PSHR16(qlsp[i],2);
204 #else
205    for (i=0;i<order;i++)
206       qlsp[i]=qlsp[i] * .00097656;
207 #endif
208 
209    for (i=0;i<order;i++)
210       qlsp[i]=lsp[i]-qlsp[i];
211 }
212 
lsp_unquant_nb(spx_lsp_t * lsp,int order,SpeexBits * bits)213 void lsp_unquant_nb(spx_lsp_t *lsp, int order, SpeexBits *bits)
214 {
215    int i, id;
216    for (i=0;i<order;i++)
217       lsp[i]=LSP_LINEAR(i);
218 
219 
220    id=speex_bits_unpack_unsigned(bits, 6);
221    for (i=0;i<10;i++)
222       lsp[i] = ADD32(lsp[i], LSP_DIV_256(cdbk_nb[id*10+i]));
223 
224    id=speex_bits_unpack_unsigned(bits, 6);
225    for (i=0;i<5;i++)
226       lsp[i] = ADD16(lsp[i], LSP_DIV_512(cdbk_nb_low1[id*5+i]));
227 
228    id=speex_bits_unpack_unsigned(bits, 6);
229    for (i=0;i<5;i++)
230       lsp[i] = ADD32(lsp[i], LSP_DIV_1024(cdbk_nb_low2[id*5+i]));
231 
232    id=speex_bits_unpack_unsigned(bits, 6);
233    for (i=0;i<5;i++)
234       lsp[i+5] = ADD32(lsp[i+5], LSP_DIV_512(cdbk_nb_high1[id*5+i]));
235 
236    id=speex_bits_unpack_unsigned(bits, 6);
237    for (i=0;i<5;i++)
238       lsp[i+5] = ADD32(lsp[i+5], LSP_DIV_1024(cdbk_nb_high2[id*5+i]));
239 }
240 
241 
lsp_quant_lbr(spx_lsp_t * lsp,spx_lsp_t * qlsp,int order,SpeexBits * bits)242 void lsp_quant_lbr(spx_lsp_t *lsp, spx_lsp_t *qlsp, int order, SpeexBits *bits)
243 {
244    int i;
245    int id;
246    spx_word16_t quant_weight[10];
247 
248    for (i=0;i<order;i++)
249       qlsp[i]=lsp[i];
250 
251    compute_quant_weights(qlsp, quant_weight, order);
252 
253    for (i=0;i<order;i++)
254       qlsp[i]=SUB16(qlsp[i],LSP_LINEAR(i));
255 #ifndef FIXED_POINT
256    for (i=0;i<order;i++)
257       qlsp[i]=qlsp[i]*LSP_SCALE;
258 #endif
259    id = lsp_quant(qlsp, cdbk_nb, NB_CDBK_SIZE, order);
260    speex_bits_pack(bits, id, 6);
261 
262    for (i=0;i<order;i++)
263       qlsp[i]*=2;
264 
265    id = lsp_weight_quant(qlsp, quant_weight, cdbk_nb_low1, NB_CDBK_SIZE_LOW1, 5);
266    speex_bits_pack(bits, id, 6);
267 
268    id = lsp_weight_quant(qlsp+5, quant_weight+5, cdbk_nb_high1, NB_CDBK_SIZE_HIGH1, 5);
269    speex_bits_pack(bits, id, 6);
270 
271 #ifdef FIXED_POINT
272    for (i=0;i<order;i++)
273       qlsp[i] = PSHR16(qlsp[i],1);
274 #else
275    for (i=0;i<order;i++)
276       qlsp[i] = qlsp[i]*0.0019531;
277 #endif
278 
279    for (i=0;i<order;i++)
280       qlsp[i]=lsp[i]-qlsp[i];
281 }
282 
lsp_unquant_lbr(spx_lsp_t * lsp,int order,SpeexBits * bits)283 void lsp_unquant_lbr(spx_lsp_t *lsp, int order, SpeexBits *bits)
284 {
285    int i, id;
286    for (i=0;i<order;i++)
287       lsp[i]=LSP_LINEAR(i);
288 
289 
290    id=speex_bits_unpack_unsigned(bits, 6);
291    for (i=0;i<10;i++)
292       lsp[i] += LSP_DIV_256(cdbk_nb[id*10+i]);
293 
294    id=speex_bits_unpack_unsigned(bits, 6);
295    for (i=0;i<5;i++)
296       lsp[i] += LSP_DIV_512(cdbk_nb_low1[id*5+i]);
297 
298    id=speex_bits_unpack_unsigned(bits, 6);
299    for (i=0;i<5;i++)
300       lsp[i+5] += LSP_DIV_512(cdbk_nb_high1[id*5+i]);
301 
302 }
303 
304 
305 #ifdef DISABLE_WIDEBAND
lsp_quant_high(spx_lsp_t * lsp,spx_lsp_t * qlsp,int order,SpeexBits * bits)306 void lsp_quant_high(spx_lsp_t *lsp, spx_lsp_t *qlsp, int order, SpeexBits *bits)
307 {
308    speex_fatal("Wideband and Ultra-wideband are disabled");
309 }
lsp_unquant_high(spx_lsp_t * lsp,int order,SpeexBits * bits)310 void lsp_unquant_high(spx_lsp_t *lsp, int order, SpeexBits *bits)
311 {
312    speex_fatal("Wideband and Ultra-wideband are disabled");
313 }
314 #else
315 extern const signed char high_lsp_cdbk[];
316 extern const signed char high_lsp_cdbk2[];
317 
318 
lsp_quant_high(spx_lsp_t * lsp,spx_lsp_t * qlsp,int order,SpeexBits * bits)319 void lsp_quant_high(spx_lsp_t *lsp, spx_lsp_t *qlsp, int order, SpeexBits *bits)
320 {
321    int i;
322    int id;
323    spx_word16_t quant_weight[10];
324 
325    for (i=0;i<order;i++)
326       qlsp[i]=lsp[i];
327 
328    compute_quant_weights(qlsp, quant_weight, order);
329 
330    /*   quant_weight[0] = 10/(qlsp[1]-qlsp[0]);
331    quant_weight[order-1] = 10/(qlsp[order-1]-qlsp[order-2]);
332    for (i=1;i<order-1;i++)
333    {
334       tmp1 = 10/(qlsp[i]-qlsp[i-1]);
335       tmp2 = 10/(qlsp[i+1]-qlsp[i]);
336       quant_weight[i] = tmp1 > tmp2 ? tmp1 : tmp2;
337       }*/
338 
339    for (i=0;i<order;i++)
340       qlsp[i]=SUB16(qlsp[i],LSP_LINEAR_HIGH(i));
341 #ifndef FIXED_POINT
342    for (i=0;i<order;i++)
343       qlsp[i] = qlsp[i]*LSP_SCALE;
344 #endif
345    id = lsp_quant(qlsp, high_lsp_cdbk, 64, order);
346    speex_bits_pack(bits, id, 6);
347 
348    for (i=0;i<order;i++)
349       qlsp[i]*=2;
350 
351    id = lsp_weight_quant(qlsp, quant_weight, high_lsp_cdbk2, 64, order);
352    speex_bits_pack(bits, id, 6);
353 
354 #ifdef FIXED_POINT
355    for (i=0;i<order;i++)
356       qlsp[i] = PSHR16(qlsp[i],1);
357 #else
358    for (i=0;i<order;i++)
359       qlsp[i] = qlsp[i]*0.0019531;
360 #endif
361 
362    for (i=0;i<order;i++)
363       qlsp[i]=lsp[i]-qlsp[i];
364 }
365 
lsp_unquant_high(spx_lsp_t * lsp,int order,SpeexBits * bits)366 void lsp_unquant_high(spx_lsp_t *lsp, int order, SpeexBits *bits)
367 {
368 
369    int i, id;
370    for (i=0;i<order;i++)
371       lsp[i]=LSP_LINEAR_HIGH(i);
372 
373 
374    id=speex_bits_unpack_unsigned(bits, 6);
375    for (i=0;i<order;i++)
376       lsp[i] += LSP_DIV_256(high_lsp_cdbk[id*order+i]);
377 
378 
379    id=speex_bits_unpack_unsigned(bits, 6);
380    for (i=0;i<order;i++)
381       lsp[i] += LSP_DIV_512(high_lsp_cdbk2[id*order+i]);
382 }
383 
384 #endif
385 
386