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