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