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