f9a0d498e44d9a678382ce1f5fe62475da388d54
[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, 11));
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    int q=0;
92    for (n=0;n<len;n++,q++)
93       t[n] = SUB32(t[n],MULT16_16_Q11_32(g,r[q]));
94 }
95 #endif
96
97
98
99 static void split_cb_search_shape_sign_N1(
100 spx_sig_t target[],                     /* target vector */
101 spx_coef_t ak[],                        /* LPCs for this subframe */
102 spx_coef_t awk1[],                      /* Weighted LPCs for this subframe */
103 spx_coef_t awk2[],                      /* Weighted LPCs for this subframe */
104 const void *par,                      /* Codebook/search parameters*/
105 int   p,                        /* number of LPC coeffs */
106 int   nsf,                      /* number of samples in subframe */
107 spx_sig_t *exc,
108 spx_word16_t *r,
109 SpeexBits *bits,
110 char *stack,
111 int   complexity,
112 int   update_target
113 )
114 {
115    int i,j,m,q;
116 #ifndef FIXED_POINT
117    int n;
118 #endif
119    VARDECL(spx_word16_t *resp);
120 #ifdef _USE_SSE
121    VARDECL(__m128 *resp2);
122    VARDECL(__m128 *E);
123 #else
124    spx_word16_t *resp2;
125    VARDECL(spx_word32_t *E);
126 #endif
127    VARDECL(spx_word16_t *t);
128    VARDECL(spx_sig_t *e);
129    const signed char *shape_cb;
130    int shape_cb_size, subvect_size, nb_subvect;
131    const split_cb_params *params;
132    int N=2;
133    int best_index;
134    spx_word32_t best_dist;
135    int have_sign;
136    N=complexity;
137    if (N>10)
138       N=10;
139    if (N<1)
140       N=1;
141    
142    params = (const split_cb_params *) par;
143    subvect_size = params->subvect_size;
144    nb_subvect = params->nb_subvect;
145    shape_cb_size = 1<<params->shape_bits;
146    shape_cb = params->shape_cb;
147    have_sign = params->have_sign;
148    ALLOC(resp, shape_cb_size*subvect_size, spx_word16_t);
149 #ifdef _USE_SSE
150    ALLOC(resp2, (shape_cb_size*subvect_size)>>2, __m128);
151    ALLOC(E, shape_cb_size>>2, __m128);
152 #else
153    resp2 = resp;
154    ALLOC(E, shape_cb_size, spx_word32_t);
155 #endif
156    ALLOC(t, nsf, spx_word16_t);
157    ALLOC(e, nsf, spx_sig_t);
158    
159    /* FIXME: make that adaptive? */
160    for (i=0;i<nsf;i++)
161       t[i]=EXTRACT16(PSHR32(target[i],6));
162
163    compute_weighted_codebook(shape_cb, r, resp, resp2, E, shape_cb_size, subvect_size, stack);
164
165    for (i=0;i<nb_subvect;i++)
166    {
167       spx_word16_t *x=t+subvect_size*i;
168       /*Find new n-best based on previous n-best j*/
169       if (have_sign)
170          vq_nbest_sign(x, resp2, subvect_size, shape_cb_size, E, 1, &best_index, &best_dist, stack);
171       else
172          vq_nbest(x, resp2, subvect_size, shape_cb_size, E, 1, &best_index, &best_dist, stack);
173       
174       speex_bits_pack(bits,best_index,params->shape_bits+have_sign);
175       
176       {
177          int rind;
178          spx_word16_t *res;
179          spx_word16_t sign=1;
180          rind = best_index;
181          if (rind>=shape_cb_size)
182          {
183             sign=-1;
184             rind-=shape_cb_size;
185          }
186          res = resp+rind*subvect_size;
187          if (sign>0)
188             for (m=0;m<subvect_size;m++)
189                t[subvect_size*i+m] = SUB16(t[subvect_size*i+m], res[m]);
190          else
191             for (m=0;m<subvect_size;m++)
192                t[subvect_size*i+m] = ADD16(t[subvect_size*i+m], res[m]);
193
194 #ifdef FIXED_POINT
195          if (sign)
196          {
197             for (j=0;j<subvect_size;j++)
198                e[subvect_size*i+j]=SHL32(EXTEND32(shape_cb[rind*subvect_size+j]),SIG_SHIFT-5);
199          } else {
200             for (j=0;j<subvect_size;j++)
201                e[subvect_size*i+j]=NEG32(SHL32(EXTEND32(shape_cb[rind*subvect_size+j]),SIG_SHIFT-5));
202          }
203 #else
204          for (j=0;j<subvect_size;j++)
205             e[subvect_size*i+j]=sign*0.03125*shape_cb[rind*subvect_size+j];
206 #endif
207       
208       }
209             
210       for (m=0;m<subvect_size;m++)
211       {
212          spx_word16_t g;
213          int rind;
214          spx_word16_t sign=1;
215          rind = best_index;
216          if (rind>=shape_cb_size)
217          {
218             sign=-1;
219             rind-=shape_cb_size;
220          }
221          
222          q=subvect_size-m;
223 #ifdef FIXED_POINT
224          g=sign*shape_cb[rind*subvect_size+m];
225          target_update(t+subvect_size*(i+1), g, r+q, nsf-subvect_size*(i+1));
226 #else
227          g=sign*0.03125*shape_cb[rind*subvect_size+m];
228          /*FIXME: I think that one too can be replaced by target_update */
229          for (n=subvect_size*(i+1);n<nsf;n++,q++)
230             t[n] = SUB32(t[n],g*r[q]);
231 #endif
232       }
233    }
234
235    /* Update excitation */
236    /* FIXME: We could update the excitation directly above */
237    for (j=0;j<nsf;j++)
238       exc[j]=ADD32(exc[j],e[j]);
239    
240    /* Update target: only update target if necessary */
241    if (update_target)
242    {
243       VARDECL(spx_sig_t *r2);
244       ALLOC(r2, nsf, spx_sig_t);
245       syn_percep_zero(e, ak, awk1, awk2, r2, nsf,p, stack);
246       for (j=0;j<nsf;j++)
247          target[j]=SUB32(target[j],r2[j]);
248    }
249 }
250
251
252
253 void split_cb_search_shape_sign(
254 spx_sig_t target[],                     /* target vector */
255 spx_coef_t ak[],                        /* LPCs for this subframe */
256 spx_coef_t awk1[],                      /* Weighted LPCs for this subframe */
257 spx_coef_t awk2[],                      /* Weighted LPCs for this subframe */
258 const void *par,                      /* Codebook/search parameters*/
259 int   p,                        /* number of LPC coeffs */
260 int   nsf,                      /* number of samples in subframe */
261 spx_sig_t *exc,
262 spx_word16_t *r,
263 SpeexBits *bits,
264 char *stack,
265 int   complexity,
266 int   update_target
267 )
268 {
269    int i,j,k,m,n,q;
270    VARDECL(spx_word16_t *resp);
271 #ifdef _USE_SSE
272    VARDECL(__m128 *resp2);
273    VARDECL(__m128 *E);
274 #else
275    spx_word16_t *resp2;
276    VARDECL(spx_word32_t *E);
277 #endif
278    VARDECL(spx_word16_t *t);
279    VARDECL(spx_sig_t *e);
280    VARDECL(spx_sig_t *r2);
281    VARDECL(spx_word16_t *tmp);
282    VARDECL(spx_word32_t *ndist);
283    VARDECL(spx_word32_t *odist);
284    VARDECL(int *itmp);
285    VARDECL(spx_word16_t **ot2);
286    VARDECL(spx_word16_t **nt2);
287    spx_word16_t **ot, **nt;
288    VARDECL(int **nind);
289    VARDECL(int **oind);
290    VARDECL(int *ind);
291    const signed char *shape_cb;
292    int shape_cb_size, subvect_size, nb_subvect;
293    const split_cb_params *params;
294    int N=2;
295    VARDECL(int *best_index);
296    VARDECL(spx_word32_t *best_dist);
297    VARDECL(int *best_nind);
298    VARDECL(int *best_ntarget);
299    int have_sign;
300    N=complexity;
301    if (N>10)
302       N=10;
303    if (N<1)
304       N=1;
305    
306    if (N==1)
307    {
308       split_cb_search_shape_sign_N1(target,ak,awk1,awk2,par,p,nsf,exc,r,bits,stack,complexity,update_target);
309       return;
310    }
311    ALLOC(ot2, N, spx_word16_t*);
312    ALLOC(nt2, N, spx_word16_t*);
313    ALLOC(oind, N, int*);
314    ALLOC(nind, N, int*);
315
316    params = (const split_cb_params *) par;
317    subvect_size = params->subvect_size;
318    nb_subvect = params->nb_subvect;
319    shape_cb_size = 1<<params->shape_bits;
320    shape_cb = params->shape_cb;
321    have_sign = params->have_sign;
322    ALLOC(resp, shape_cb_size*subvect_size, spx_word16_t);
323 #ifdef _USE_SSE
324    ALLOC(resp2, (shape_cb_size*subvect_size)>>2, __m128);
325    ALLOC(E, shape_cb_size>>2, __m128);
326 #else
327    resp2 = resp;
328    ALLOC(E, shape_cb_size, spx_word32_t);
329 #endif
330    ALLOC(t, nsf, spx_word16_t);
331    ALLOC(e, nsf, spx_sig_t);
332    ALLOC(r2, nsf, spx_sig_t);
333    ALLOC(ind, nb_subvect, int);
334
335    ALLOC(tmp, 2*N*nsf, spx_word16_t);
336    for (i=0;i<N;i++)
337    {
338       ot2[i]=tmp+2*i*nsf;
339       nt2[i]=tmp+(2*i+1)*nsf;
340    }
341    ot=ot2;
342    nt=nt2;
343    ALLOC(best_index, N, int);
344    ALLOC(best_dist, N, spx_word32_t);
345    ALLOC(best_nind, N, int);
346    ALLOC(best_ntarget, N, int);
347    ALLOC(ndist, N, spx_word32_t);
348    ALLOC(odist, N, spx_word32_t);
349    
350    ALLOC(itmp, 2*N*nb_subvect, int);
351    for (i=0;i<N;i++)
352    {
353       nind[i]=itmp+2*i*nb_subvect;
354       oind[i]=itmp+(2*i+1)*nb_subvect;
355    }
356    
357    /* FIXME: make that adaptive? */
358    for (i=0;i<nsf;i++)
359       t[i]=EXTRACT16(PSHR32(target[i],6));
360
361    for (j=0;j<N;j++)
362       speex_move(&ot[j][0], t, nsf*sizeof(spx_word16_t));
363
364    /* Pre-compute codewords response and energy */
365    compute_weighted_codebook(shape_cb, r, resp, resp2, E, shape_cb_size, subvect_size, stack);
366
367    for (j=0;j<N;j++)
368       odist[j]=0;
369    
370    /*For all subvectors*/
371    for (i=0;i<nb_subvect;i++)
372    {
373       /*"erase" nbest list*/
374       for (j=0;j<N;j++)
375          ndist[j]=VERY_LARGE32;
376
377       /*For all n-bests of previous subvector*/
378       for (j=0;j<N;j++)
379       {
380          spx_word16_t *x=ot[j]+subvect_size*i;
381          spx_word32_t tener = 0;
382          for (m=0;m<subvect_size;m++)
383             tener = MAC16_16(tener, x[m],x[m]);
384 #ifdef FIXED_POINT
385          tener = SHR32(tener,1);
386 #else
387          tener *= .5;
388 #endif
389          /*Find new n-best based on previous n-best j*/
390          if (have_sign)
391             vq_nbest_sign(x, resp2, subvect_size, shape_cb_size, E, N, best_index, best_dist, stack);
392          else
393             vq_nbest(x, resp2, subvect_size, shape_cb_size, E, N, best_index, best_dist, stack);
394
395          /*For all new n-bests*/
396          for (k=0;k<N;k++)
397          {
398             /* Compute total distance (including previous sub-vectors */
399             spx_word32_t err = ADD32(ADD32(odist[j],best_dist[k]),tener);
400             
401             /*update n-best list*/
402             if (err<ndist[N-1])
403             {
404                for (m=0;m<N;m++)
405                {
406                   if (err < ndist[m])
407                   {
408                      for (n=N-1;n>m;n--)
409                      {
410                         ndist[n] = ndist[n-1];
411                         best_nind[n] = best_nind[n-1];
412                         best_ntarget[n] = best_ntarget[n-1];
413                      }
414                      ndist[m] = err;
415                      best_nind[n] = best_index[k];
416                      best_ntarget[n] = j;
417                      break;
418                   }
419                }
420             }
421          }
422          if (i==0)
423             break;
424       }
425       for (j=0;j<N;j++)
426       {
427          //spx_word16_t *ct = ot[best_ntarget[j]];
428          
429          /*previous target (we don't care what happened before*/
430          for (m=(i+1)*subvect_size;m<nsf;m++)
431             nt[j][m]=ot[best_ntarget[j]][m];
432          
433          /* New code: update the rest of the target only if it's worth it */
434          for (m=0;m<subvect_size;m++)
435          {
436             spx_word16_t g;
437             int rind;
438             spx_word16_t sign=1;
439             rind = best_nind[j];
440             if (rind>=shape_cb_size)
441             {
442                sign=-1;
443                rind-=shape_cb_size;
444             }
445
446             q=subvect_size-m;
447 #ifdef FIXED_POINT
448             g=sign*shape_cb[rind*subvect_size+m];
449             target_update(nt[j]+subvect_size*(i+1), g, r+q, nsf-subvect_size*(i+1));
450 #else
451             g=sign*0.03125*shape_cb[rind*subvect_size+m];
452             /*FIXME: I think that one too can be replaced by target_update */
453             for (n=subvect_size*(i+1);n<nsf;n++,q++)
454                nt[j][n] = SUB32(nt[j][n],g*r[q]);
455 #endif
456          }
457
458          for (q=0;q<nb_subvect;q++)
459             nind[j][q]=oind[best_ntarget[j]][q];
460          nind[j][i]=best_nind[j];
461       }
462
463       /*update old-new data*/
464       /* just swap pointers instead of a long copy */
465       {
466          spx_word16_t **tmp2;
467          tmp2=ot;
468          ot=nt;
469          nt=tmp2;
470       }
471       for (j=0;j<N;j++)
472          for (m=0;m<nb_subvect;m++)
473             oind[j][m]=nind[j][m];
474       for (j=0;j<N;j++)
475          odist[j]=ndist[j];
476    }
477
478    /*save indices*/
479    for (i=0;i<nb_subvect;i++)
480    {
481       ind[i]=nind[0][i];
482       speex_bits_pack(bits,ind[i],params->shape_bits+have_sign);
483    }
484    
485    /* Put everything back together */
486    for (i=0;i<nb_subvect;i++)
487    {
488       int rind;
489       spx_word16_t sign=1;
490       rind = ind[i];
491       if (rind>=shape_cb_size)
492       {
493          sign=-1;
494          rind-=shape_cb_size;
495       }
496 #ifdef FIXED_POINT
497       if (sign==1)
498       {
499          for (j=0;j<subvect_size;j++)
500             e[subvect_size*i+j]=SHL32(EXTEND32(shape_cb[rind*subvect_size+j]),SIG_SHIFT-5);
501       } else {
502          for (j=0;j<subvect_size;j++)
503             e[subvect_size*i+j]=NEG32(SHL32(EXTEND32(shape_cb[rind*subvect_size+j]),SIG_SHIFT-5));
504       }
505 #else
506       for (j=0;j<subvect_size;j++)
507          e[subvect_size*i+j]=sign*0.03125*shape_cb[rind*subvect_size+j];
508 #endif
509    }   
510    /* Update excitation */
511    for (j=0;j<nsf;j++)
512       exc[j]=ADD32(exc[j],e[j]);
513    
514    /* Update target: only update target if necessary */
515    if (update_target)
516    {
517       syn_percep_zero(e, ak, awk1, awk2, r2, nsf,p, stack);
518       for (j=0;j<nsf;j++)
519          target[j]=SUB32(target[j],r2[j]);
520    }
521 }
522
523
524 void split_cb_shape_sign_unquant(
525 spx_sig_t *exc,
526 const void *par,                      /* non-overlapping codebook */
527 int   nsf,                      /* number of samples in subframe */
528 SpeexBits *bits,
529 char *stack
530 )
531 {
532    int i,j;
533    VARDECL(int *ind);
534    VARDECL(int *signs);
535    const signed char *shape_cb;
536    int shape_cb_size, subvect_size, nb_subvect;
537    const split_cb_params *params;
538    int have_sign;
539
540    params = (const split_cb_params *) par;
541    subvect_size = params->subvect_size;
542    nb_subvect = params->nb_subvect;
543    shape_cb_size = 1<<params->shape_bits;
544    shape_cb = params->shape_cb;
545    have_sign = params->have_sign;
546
547    ALLOC(ind, nb_subvect, int);
548    ALLOC(signs, nb_subvect, int);
549
550    /* Decode codewords and gains */
551    for (i=0;i<nb_subvect;i++)
552    {
553       if (have_sign)
554          signs[i] = speex_bits_unpack_unsigned(bits, 1);
555       else
556          signs[i] = 0;
557       ind[i] = speex_bits_unpack_unsigned(bits, params->shape_bits);
558    }
559    /* Compute decoded excitation */
560    for (i=0;i<nb_subvect;i++)
561    {
562       spx_word16_t s=1;
563       if (signs[i])
564          s=-1;
565 #ifdef FIXED_POINT
566       if (s==1)
567       {
568          for (j=0;j<subvect_size;j++)
569             exc[subvect_size*i+j]=SHL32(EXTEND32(shape_cb[ind[i]*subvect_size+j]),SIG_SHIFT-5);
570       } else {
571          for (j=0;j<subvect_size;j++)
572             exc[subvect_size*i+j]=NEG32(SHL32(EXTEND32(shape_cb[ind[i]*subvect_size+j]),SIG_SHIFT-5));
573       }
574 #else
575       for (j=0;j<subvect_size;j++)
576          exc[subvect_size*i+j]+=s*0.03125*shape_cb[ind[i]*subvect_size+j];      
577 #endif
578    }
579 }
580
581 void noise_codebook_quant(
582 spx_sig_t target[],                     /* target vector */
583 spx_coef_t ak[],                        /* LPCs for this subframe */
584 spx_coef_t awk1[],                      /* Weighted LPCs for this subframe */
585 spx_coef_t awk2[],                      /* Weighted LPCs for this subframe */
586 const void *par,                      /* Codebook/search parameters*/
587 int   p,                        /* number of LPC coeffs */
588 int   nsf,                      /* number of samples in subframe */
589 spx_sig_t *exc,
590 spx_word16_t *r,
591 SpeexBits *bits,
592 char *stack,
593 int   complexity,
594 int   update_target
595 )
596 {
597    int i;
598    VARDECL(spx_sig_t *tmp);
599    ALLOC(tmp, nsf, spx_sig_t);
600    residue_percep_zero(target, ak, awk1, awk2, tmp, nsf, p, stack);
601
602    for (i=0;i<nsf;i++)
603       exc[i]+=tmp[i];
604    for (i=0;i<nsf;i++)
605       target[i]=0;
606
607 }
608
609
610 void noise_codebook_unquant(
611 spx_sig_t *exc,
612 const void *par,                      /* non-overlapping codebook */
613 int   nsf,                      /* number of samples in subframe */
614 SpeexBits *bits,
615 char *stack
616 )
617 {
618    speex_rand_vec(1, exc, nsf);
619 }