16-bit clean shift in lsp_to_lpc()
[speexdsp.git] / libspeex / cb_search.c
1 /* Copyright (C) 2002 Jean-Marc Valin 
2    File: cb_search.c
3
4    Redistribution and use in source and binary forms, with or without
5    modification, are permitted provided that the following conditions
6    are met:
7    
8    - Redistributions of source code must retain the above copyright
9    notice, this list of conditions and the following disclaimer.
10    
11    - Redistributions in binary form must reproduce the above copyright
12    notice, this list of conditions and the following disclaimer in the
13    documentation and/or other materials provided with the distribution.
14    
15    - Neither the name of the Xiph.org Foundation nor the names of its
16    contributors may be used to endorse or promote products derived from
17    this software without specific prior written permission.
18    
19    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
22    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
23    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
24    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
25    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
26    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
27    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
28    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
29    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30 */
31
32 #ifdef HAVE_CONFIG_H
33 #include "config.h"
34 #endif
35
36 #include "cb_search.h"
37 #include "filters.h"
38 #include "stack_alloc.h"
39 #include "vq.h"
40 #include "misc.h"
41
42 #ifdef _USE_SSE
43 #include "cb_search_sse.h"
44 #elif defined(ARM4_ASM) || defined(ARM5E_ASM)
45 #include "cb_search_arm4.h"
46 #elif defined(BFIN_ASM)
47 #include "cb_search_bfin.h"
48 #endif
49
50 #ifndef OVERRIDE_COMPUTE_WEIGHTED_CODEBOOK
51 static void compute_weighted_codebook(const signed char *shape_cb, const spx_word16_t *r, spx_word16_t *resp, spx_word16_t *resp2, spx_word32_t *E, int shape_cb_size, int subvect_size, char *stack)
52 {
53    int i, j, k;
54    VARDECL(spx_word16_t *shape);
55    ALLOC(shape, subvect_size, spx_word16_t);
56    for (i=0;i<shape_cb_size;i++)
57    {
58       spx_word16_t *res;
59       
60       res = resp+i*subvect_size;
61       for (k=0;k<subvect_size;k++)
62          shape[k] = (spx_word16_t)shape_cb[i*subvect_size+k];
63       E[i]=0;
64
65       /* Compute codeword response using convolution with impulse response */
66       for(j=0;j<subvect_size;j++)
67       {
68          spx_word32_t resj=0;
69          spx_word16_t res16;
70          for (k=0;k<=j;k++)
71             resj = MAC16_16(resj,shape[k],r[j-k]);
72 #ifdef FIXED_POINT
73          res16 = EXTRACT16(SHR32(resj, 13));
74 #else
75          res16 = 0.03125f*resj;
76 #endif
77          /* Compute codeword energy */
78          E[i]=MAC16_16(E[i],res16,res16);
79          res[j] = res16;
80          /*printf ("%d\n", (int)res[j]);*/
81       }
82    }
83
84 }
85 #endif
86
87 #ifndef OVERRIDE_TARGET_UPDATE
88 static inline void target_update(spx_word16_t *t, spx_word16_t g, spx_word16_t *r, int len)
89 {
90    int n;
91    for (n=0;n<len;n++)
92       t[n] = SUB16(t[n],PSHR32(MULT16_16(g,r[n]),13));
93 }
94 #endif
95
96
97
98 static void split_cb_search_shape_sign_N1(
99 spx_word16_t target[],                  /* target vector */
100 spx_coef_t ak[],                        /* LPCs for this subframe */
101 spx_coef_t awk1[],                      /* Weighted LPCs for this subframe */
102 spx_coef_t awk2[],                      /* Weighted LPCs for this subframe */
103 const void *par,                      /* Codebook/search parameters*/
104 int   p,                        /* number of LPC coeffs */
105 int   nsf,                      /* number of samples in subframe */
106 spx_sig_t *exc,
107 spx_word16_t *r,
108 SpeexBits *bits,
109 char *stack,
110 int   complexity,
111 int   update_target
112 )
113 {
114    int i,j,m,q;
115    VARDECL(spx_word16_t *resp);
116 #ifdef _USE_SSE
117    VARDECL(__m128 *resp2);
118    VARDECL(__m128 *E);
119 #else
120    spx_word16_t *resp2;
121    VARDECL(spx_word32_t *E);
122 #endif
123    VARDECL(spx_word16_t *t);
124    VARDECL(spx_sig_t *e);
125    const signed char *shape_cb;
126    int shape_cb_size, subvect_size, nb_subvect;
127    const split_cb_params *params;
128    int N=2;
129    int best_index;
130    spx_word32_t best_dist;
131    int have_sign;
132    N=complexity;
133    if (N>10)
134       N=10;
135    if (N<1)
136       N=1;
137    
138    params = (const split_cb_params *) par;
139    subvect_size = params->subvect_size;
140    nb_subvect = params->nb_subvect;
141    shape_cb_size = 1<<params->shape_bits;
142    shape_cb = params->shape_cb;
143    have_sign = params->have_sign;
144    ALLOC(resp, shape_cb_size*subvect_size, spx_word16_t);
145 #ifdef _USE_SSE
146    ALLOC(resp2, (shape_cb_size*subvect_size)>>2, __m128);
147    ALLOC(E, shape_cb_size>>2, __m128);
148 #else
149    resp2 = resp;
150    ALLOC(E, shape_cb_size, spx_word32_t);
151 #endif
152    ALLOC(t, nsf, spx_word16_t);
153    ALLOC(e, nsf, spx_sig_t);
154    
155    /* FIXME: make that adaptive? */
156    for (i=0;i<nsf;i++)
157       t[i]=target[i];
158
159    compute_weighted_codebook(shape_cb, r, resp, resp2, E, shape_cb_size, subvect_size, stack);
160
161    for (i=0;i<nb_subvect;i++)
162    {
163       spx_word16_t *x=t+subvect_size*i;
164       /*Find new n-best based on previous n-best j*/
165       if (have_sign)
166          vq_nbest_sign(x, resp2, subvect_size, shape_cb_size, E, 1, &best_index, &best_dist, stack);
167       else
168          vq_nbest(x, resp2, subvect_size, shape_cb_size, E, 1, &best_index, &best_dist, stack);
169       
170       speex_bits_pack(bits,best_index,params->shape_bits+have_sign);
171       
172       {
173          int rind;
174          spx_word16_t *res;
175          spx_word16_t sign=1;
176          rind = best_index;
177          if (rind>=shape_cb_size)
178          {
179             sign=-1;
180             rind-=shape_cb_size;
181          }
182          res = resp+rind*subvect_size;
183          if (sign>0)
184             for (m=0;m<subvect_size;m++)
185                t[subvect_size*i+m] = SUB16(t[subvect_size*i+m], res[m]);
186          else
187             for (m=0;m<subvect_size;m++)
188                t[subvect_size*i+m] = ADD16(t[subvect_size*i+m], res[m]);
189
190 #ifdef FIXED_POINT
191          if (sign)
192          {
193             for (j=0;j<subvect_size;j++)
194                e[subvect_size*i+j]=SHL32(EXTEND32(shape_cb[rind*subvect_size+j]),SIG_SHIFT-5);
195          } else {
196             for (j=0;j<subvect_size;j++)
197                e[subvect_size*i+j]=NEG32(SHL32(EXTEND32(shape_cb[rind*subvect_size+j]),SIG_SHIFT-5));
198          }
199 #else
200          for (j=0;j<subvect_size;j++)
201             e[subvect_size*i+j]=sign*0.03125*shape_cb[rind*subvect_size+j];
202 #endif
203       
204       }
205             
206       for (m=0;m<subvect_size;m++)
207       {
208          spx_word16_t g;
209          int rind;
210          spx_word16_t sign=1;
211          rind = best_index;
212          if (rind>=shape_cb_size)
213          {
214             sign=-1;
215             rind-=shape_cb_size;
216          }
217          
218          q=subvect_size-m;
219 #ifdef FIXED_POINT
220          g=sign*shape_cb[rind*subvect_size+m];
221 #else
222          g=sign*0.03125*shape_cb[rind*subvect_size+m];
223 #endif
224          target_update(t+subvect_size*(i+1), g, r+q, nsf-subvect_size*(i+1));
225       }
226    }
227
228    /* Update excitation */
229    /* FIXME: We could update the excitation directly above */
230    for (j=0;j<nsf;j++)
231       exc[j]=ADD32(exc[j],e[j]);
232    
233    /* Update target: only update target if necessary */
234    if (update_target)
235    {
236       VARDECL(spx_sig_t *r2);
237       ALLOC(r2, nsf, spx_sig_t);
238       syn_percep_zero(e, ak, awk1, awk2, r2, nsf,p, stack);
239       for (j=0;j<nsf;j++)
240          target[j]=SUB16(target[j],EXTRACT16(PSHR32(r2[j],8)));
241    }
242 }
243
244
245
246 void split_cb_search_shape_sign(
247 spx_word16_t target[],                  /* target vector */
248 spx_coef_t ak[],                        /* LPCs for this subframe */
249 spx_coef_t awk1[],                      /* Weighted LPCs for this subframe */
250 spx_coef_t awk2[],                      /* Weighted LPCs for this subframe */
251 const void *par,                      /* Codebook/search parameters*/
252 int   p,                        /* number of LPC coeffs */
253 int   nsf,                      /* number of samples in subframe */
254 spx_sig_t *exc,
255 spx_word16_t *r,
256 SpeexBits *bits,
257 char *stack,
258 int   complexity,
259 int   update_target
260 )
261 {
262    int i,j,k,m,n,q;
263    VARDECL(spx_word16_t *resp);
264 #ifdef _USE_SSE
265    VARDECL(__m128 *resp2);
266    VARDECL(__m128 *E);
267 #else
268    spx_word16_t *resp2;
269    VARDECL(spx_word32_t *E);
270 #endif
271    VARDECL(spx_word16_t *t);
272    VARDECL(spx_sig_t *e);
273    VARDECL(spx_sig_t *r2);
274    VARDECL(spx_word16_t *tmp);
275    VARDECL(spx_word32_t *ndist);
276    VARDECL(spx_word32_t *odist);
277    VARDECL(int *itmp);
278    VARDECL(spx_word16_t **ot2);
279    VARDECL(spx_word16_t **nt2);
280    spx_word16_t **ot, **nt;
281    VARDECL(int **nind);
282    VARDECL(int **oind);
283    VARDECL(int *ind);
284    const signed char *shape_cb;
285    int shape_cb_size, subvect_size, nb_subvect;
286    const split_cb_params *params;
287    int N=2;
288    VARDECL(int *best_index);
289    VARDECL(spx_word32_t *best_dist);
290    VARDECL(int *best_nind);
291    VARDECL(int *best_ntarget);
292    int have_sign;
293    N=complexity;
294    if (N>10)
295       N=10;
296    if (N<1)
297       N=1;
298    
299    if (N==1)
300    {
301       split_cb_search_shape_sign_N1(target,ak,awk1,awk2,par,p,nsf,exc,r,bits,stack,complexity,update_target);
302       return;
303    }
304    ALLOC(ot2, N, spx_word16_t*);
305    ALLOC(nt2, N, spx_word16_t*);
306    ALLOC(oind, N, int*);
307    ALLOC(nind, N, int*);
308
309    params = (const split_cb_params *) par;
310    subvect_size = params->subvect_size;
311    nb_subvect = params->nb_subvect;
312    shape_cb_size = 1<<params->shape_bits;
313    shape_cb = params->shape_cb;
314    have_sign = params->have_sign;
315    ALLOC(resp, shape_cb_size*subvect_size, spx_word16_t);
316 #ifdef _USE_SSE
317    ALLOC(resp2, (shape_cb_size*subvect_size)>>2, __m128);
318    ALLOC(E, shape_cb_size>>2, __m128);
319 #else
320    resp2 = resp;
321    ALLOC(E, shape_cb_size, spx_word32_t);
322 #endif
323    ALLOC(t, nsf, spx_word16_t);
324    ALLOC(e, nsf, spx_sig_t);
325    ALLOC(r2, nsf, spx_sig_t);
326    ALLOC(ind, nb_subvect, int);
327
328    ALLOC(tmp, 2*N*nsf, spx_word16_t);
329    for (i=0;i<N;i++)
330    {
331       ot2[i]=tmp+2*i*nsf;
332       nt2[i]=tmp+(2*i+1)*nsf;
333    }
334    ot=ot2;
335    nt=nt2;
336    ALLOC(best_index, N, int);
337    ALLOC(best_dist, N, spx_word32_t);
338    ALLOC(best_nind, N, int);
339    ALLOC(best_ntarget, N, int);
340    ALLOC(ndist, N, spx_word32_t);
341    ALLOC(odist, N, spx_word32_t);
342    
343    ALLOC(itmp, 2*N*nb_subvect, int);
344    for (i=0;i<N;i++)
345    {
346       nind[i]=itmp+2*i*nb_subvect;
347       oind[i]=itmp+(2*i+1)*nb_subvect;
348    }
349    
350    /* FIXME: make that adaptive? */
351    for (i=0;i<nsf;i++)
352       t[i]=target[i];
353
354    for (j=0;j<N;j++)
355       speex_move(&ot[j][0], t, nsf*sizeof(spx_word16_t));
356
357    /* Pre-compute codewords response and energy */
358    compute_weighted_codebook(shape_cb, r, resp, resp2, E, shape_cb_size, subvect_size, stack);
359
360    for (j=0;j<N;j++)
361       odist[j]=0;
362    
363    /*For all subvectors*/
364    for (i=0;i<nb_subvect;i++)
365    {
366       /*"erase" nbest list*/
367       for (j=0;j<N;j++)
368          ndist[j]=VERY_LARGE32;
369
370       /*For all n-bests of previous subvector*/
371       for (j=0;j<N;j++)
372       {
373          spx_word16_t *x=ot[j]+subvect_size*i;
374          spx_word32_t tener = 0;
375          for (m=0;m<subvect_size;m++)
376             tener = MAC16_16(tener, x[m],x[m]);
377 #ifdef FIXED_POINT
378          tener = SHR32(tener,1);
379 #else
380          tener *= .5;
381 #endif
382          /*Find new n-best based on previous n-best j*/
383          if (have_sign)
384             vq_nbest_sign(x, resp2, subvect_size, shape_cb_size, E, N, best_index, best_dist, stack);
385          else
386             vq_nbest(x, resp2, subvect_size, shape_cb_size, E, N, best_index, best_dist, stack);
387
388          /*For all new n-bests*/
389          for (k=0;k<N;k++)
390          {
391             /* Compute total distance (including previous sub-vectors */
392             spx_word32_t err = ADD32(ADD32(odist[j],best_dist[k]),tener);
393             
394             /*update n-best list*/
395             if (err<ndist[N-1])
396             {
397                for (m=0;m<N;m++)
398                {
399                   if (err < ndist[m])
400                   {
401                      for (n=N-1;n>m;n--)
402                      {
403                         ndist[n] = ndist[n-1];
404                         best_nind[n] = best_nind[n-1];
405                         best_ntarget[n] = best_ntarget[n-1];
406                      }
407                      ndist[m] = err;
408                      best_nind[n] = best_index[k];
409                      best_ntarget[n] = j;
410                      break;
411                   }
412                }
413             }
414          }
415          if (i==0)
416             break;
417       }
418       for (j=0;j<N;j++)
419       {
420          /*previous target (we don't care what happened before*/
421          for (m=(i+1)*subvect_size;m<nsf;m++)
422             nt[j][m]=ot[best_ntarget[j]][m];
423          
424          /* New code: update the rest of the target only if it's worth it */
425          for (m=0;m<subvect_size;m++)
426          {
427             spx_word16_t g;
428             int rind;
429             spx_word16_t sign=1;
430             rind = best_nind[j];
431             if (rind>=shape_cb_size)
432             {
433                sign=-1;
434                rind-=shape_cb_size;
435             }
436
437             q=subvect_size-m;
438 #ifdef FIXED_POINT
439             g=sign*shape_cb[rind*subvect_size+m];
440 #else
441             g=sign*0.03125*shape_cb[rind*subvect_size+m];
442 #endif
443             target_update(nt[j]+subvect_size*(i+1), g, r+q, nsf-subvect_size*(i+1));
444          }
445
446          for (q=0;q<nb_subvect;q++)
447             nind[j][q]=oind[best_ntarget[j]][q];
448          nind[j][i]=best_nind[j];
449       }
450
451       /*update old-new data*/
452       /* just swap pointers instead of a long copy */
453       {
454          spx_word16_t **tmp2;
455          tmp2=ot;
456          ot=nt;
457          nt=tmp2;
458       }
459       for (j=0;j<N;j++)
460          for (m=0;m<nb_subvect;m++)
461             oind[j][m]=nind[j][m];
462       for (j=0;j<N;j++)
463          odist[j]=ndist[j];
464    }
465
466    /*save indices*/
467    for (i=0;i<nb_subvect;i++)
468    {
469       ind[i]=nind[0][i];
470       speex_bits_pack(bits,ind[i],params->shape_bits+have_sign);
471    }
472    
473    /* Put everything back together */
474    for (i=0;i<nb_subvect;i++)
475    {
476       int rind;
477       spx_word16_t sign=1;
478       rind = ind[i];
479       if (rind>=shape_cb_size)
480       {
481          sign=-1;
482          rind-=shape_cb_size;
483       }
484 #ifdef FIXED_POINT
485       if (sign==1)
486       {
487          for (j=0;j<subvect_size;j++)
488             e[subvect_size*i+j]=SHL32(EXTEND32(shape_cb[rind*subvect_size+j]),SIG_SHIFT-5);
489       } else {
490          for (j=0;j<subvect_size;j++)
491             e[subvect_size*i+j]=NEG32(SHL32(EXTEND32(shape_cb[rind*subvect_size+j]),SIG_SHIFT-5));
492       }
493 #else
494       for (j=0;j<subvect_size;j++)
495          e[subvect_size*i+j]=sign*0.03125*shape_cb[rind*subvect_size+j];
496 #endif
497    }   
498    /* Update excitation */
499    for (j=0;j<nsf;j++)
500       exc[j]=ADD32(exc[j],e[j]);
501    
502    /* Update target: only update target if necessary */
503    if (update_target)
504    {
505       syn_percep_zero(e, ak, awk1, awk2, r2, nsf,p, stack);
506       for (j=0;j<nsf;j++)
507          target[j]=SUB16(target[j],EXTRACT16(PSHR32(r2[j],8)));
508    }
509 }
510
511
512 void split_cb_shape_sign_unquant(
513 spx_sig_t *exc,
514 const void *par,                      /* non-overlapping codebook */
515 int   nsf,                      /* number of samples in subframe */
516 SpeexBits *bits,
517 char *stack
518 )
519 {
520    int i,j;
521    VARDECL(int *ind);
522    VARDECL(int *signs);
523    const signed char *shape_cb;
524    int shape_cb_size, subvect_size, nb_subvect;
525    const split_cb_params *params;
526    int have_sign;
527
528    params = (const split_cb_params *) par;
529    subvect_size = params->subvect_size;
530    nb_subvect = params->nb_subvect;
531    shape_cb_size = 1<<params->shape_bits;
532    shape_cb = params->shape_cb;
533    have_sign = params->have_sign;
534
535    ALLOC(ind, nb_subvect, int);
536    ALLOC(signs, nb_subvect, int);
537
538    /* Decode codewords and gains */
539    for (i=0;i<nb_subvect;i++)
540    {
541       if (have_sign)
542          signs[i] = speex_bits_unpack_unsigned(bits, 1);
543       else
544          signs[i] = 0;
545       ind[i] = speex_bits_unpack_unsigned(bits, params->shape_bits);
546    }
547    /* Compute decoded excitation */
548    for (i=0;i<nb_subvect;i++)
549    {
550       spx_word16_t s=1;
551       if (signs[i])
552          s=-1;
553 #ifdef FIXED_POINT
554       if (s==1)
555       {
556          for (j=0;j<subvect_size;j++)
557             exc[subvect_size*i+j]=SHL32(EXTEND32(shape_cb[ind[i]*subvect_size+j]),SIG_SHIFT-5);
558       } else {
559          for (j=0;j<subvect_size;j++)
560             exc[subvect_size*i+j]=NEG32(SHL32(EXTEND32(shape_cb[ind[i]*subvect_size+j]),SIG_SHIFT-5));
561       }
562 #else
563       for (j=0;j<subvect_size;j++)
564          exc[subvect_size*i+j]+=s*0.03125*shape_cb[ind[i]*subvect_size+j];      
565 #endif
566    }
567 }
568
569 void noise_codebook_quant(
570 spx_word16_t target[],                  /* target vector */
571 spx_coef_t ak[],                        /* LPCs for this subframe */
572 spx_coef_t awk1[],                      /* Weighted LPCs for this subframe */
573 spx_coef_t awk2[],                      /* Weighted LPCs for this subframe */
574 const void *par,                      /* Codebook/search parameters*/
575 int   p,                        /* number of LPC coeffs */
576 int   nsf,                      /* number of samples in subframe */
577 spx_sig_t *exc,
578 spx_word16_t *r,
579 SpeexBits *bits,
580 char *stack,
581 int   complexity,
582 int   update_target
583 )
584 {
585    int i;
586    VARDECL(spx_sig_t *tmp);
587    ALLOC(tmp, nsf, spx_sig_t);
588    for (i=0;i<nsf;i++)
589       tmp[i]=PSHR32(EXTEND32(target[i]),SIG_SHIFT);
590    residue_percep_zero(tmp, ak, awk1, awk2, tmp, nsf, p, stack);
591
592    for (i=0;i<nsf;i++)
593       exc[i]+=tmp[i];
594    for (i=0;i<nsf;i++)
595       target[i]=0;
596 }
597
598
599 void noise_codebook_unquant(
600 spx_sig_t *exc,
601 const void *par,                      /* non-overlapping codebook */
602 int   nsf,                      /* number of samples in subframe */
603 SpeexBits *bits,
604 char *stack
605 )
606 {
607    int i;
608    /* FIXME: This is bad, but I don't think the function ever gets called anyway */
609    spx_int32_t seed = 0;
610    for (i=0;i<nsf;i++)
611       exc[i]=SHL32(EXTEND32(speex_rand(1, &seed)),SIG_SHIFT);
612 }