filters_bfin.h

00001 /* Copyright (C) 2005 Analog Devices */
00002 /**
00003    @file filters_bfin.h
00004    @brief Various analysis/synthesis filters (Blackfin version)
00005 */
00006 /*
00007    Redistribution and use in source and binary forms, with or without
00008    modification, are permitted provided that the following conditions
00009    are met:
00010    
00011    - Redistributions of source code must retain the above copyright
00012    notice, this list of conditions and the following disclaimer.
00013    
00014    - Redistributions in binary form must reproduce the above copyright
00015    notice, this list of conditions and the following disclaimer in the
00016    documentation and/or other materials provided with the distribution.
00017    
00018    - Neither the name of the Xiph.org Foundation nor the names of its
00019    contributors may be used to endorse or promote products derived from
00020    this software without specific prior written permission.
00021    
00022    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
00025    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
00026    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
00027    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
00028    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
00029    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
00030    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
00031    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00032    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00033 */
00034 
00035 #define OVERRIDE_NORMALIZE16
00036 int normalize16(const spx_sig_t *x, spx_word16_t *y, spx_sig_t max_scale, int len)
00037 {
00038    spx_sig_t max_val=1;
00039    int sig_shift;
00040    __asm__ 
00041    (
00042    "%0 = 0;\n\t"
00043    "I0 = %1;\n\t"
00044    "L0 = 0;\n\t"
00045    "R1 = [I0++];\n\t"
00046    "LOOP norm_max%= LC0 = %2;\n\t"
00047    "LOOP_BEGIN norm_max%=;\n\t"
00048       "R2 = ABS R1 || R1 = [I0++];\n\t"
00049       "%0 = MAX(%0, R2);\n\t"
00050    "LOOP_END norm_max%=;\n\t"
00051    : "=&d" (max_val)
00052    : "a" (x), "a" (len)
00053    : "R1", "R2"
00054    );
00055 
00056    sig_shift=0;
00057    while (max_val>max_scale)
00058    {
00059       sig_shift++;
00060       max_val >>= 1;
00061    }
00062 
00063    __asm__ __volatile__ 
00064    (
00065    "I0 = %0;\n\t"
00066    "L0 = 0;\n\t"
00067    "P1 = %1;\n\t"
00068    "R0 = [I0++];\n\t"
00069    "LOOP norm_shift%= LC0 = %3;\n\t"
00070    "LOOP_BEGIN norm_shift%=;\n\t"
00071       "R1 = ASHIFT R0 by %2.L || R0 = [I0++];\n\t"
00072       "W[P1++] = R1;\n\t"
00073    "LOOP_END norm_shift%=;\n\t"
00074    "R1 = ASHIFT R0 by %2.L;\n\t"
00075    "W[P1++] = R1;\n\t"
00076    : : "a" (x), "a" (y), "d" (-sig_shift), "a" (len-1)
00077    : "I0", "L0", "P1", "R0", "R1", "memory"
00078    );
00079    return sig_shift;
00080 }
00081 
00082 
00083 
00084 #define OVERRIDE_FILTER_MEM16
00085 void filter_mem16(const spx_word16_t *_x, const spx_coef_t *num, const spx_coef_t *den, spx_word16_t *_y, int N, int ord, spx_mem_t *mem, char *stack)
00086 {
00087    VARDECL(spx_word32_t *xy2);
00088    VARDECL(spx_word32_t *numden_a);
00089    spx_word32_t *xy;
00090    spx_word16_t *numden;
00091    int i;
00092 
00093    ALLOC(xy2, (N+1), spx_word32_t);
00094    ALLOC(numden_a, (2*ord+2), spx_word32_t);
00095    xy = xy2+1;  
00096    numden = (spx_word16_t*) numden_a;
00097 
00098    for (i=0;i<ord;i++)
00099    {
00100       numden[2*i] = num[i];
00101       numden[2*i+1] = den[i];
00102    }
00103    __asm__ __volatile__
00104    (
00105    /* Register setup */
00106    "R0 = %5;\n\t"      /*ord */
00107    
00108    "P0 = %3;\n\t"
00109    "I0 = P0;\n\t"
00110    "B0 = P0;\n\t" /* numden */
00111    "L0 = 0;\n\t"
00112       
00113    "P2 = %0;\n\t" /* Fused xy */
00114    "I2 = P2;\n\t"
00115    "L2 = 0;\n\t"
00116    
00117    "P4 = %6;\n\t" /* mem */
00118    "P0 = %1;\n\t" /* _x */
00119    "P1 = %2;\n\t" /* _y */
00120    
00121    /* First sample */
00122    "R1 = [P4++];\n\t"
00123    "R1 <<= 3;\n\t" /* shift mem */
00124    "R1.L = R1 (RND);\n\t"
00125    "R2 = W[P0++];\n\t" /* load x[0] */
00126    "R1.L = R1.L + R2.L;\n\t"
00127    "W[P1++] = R1;\n\t" /* store y[0] */
00128    "R2 = PACK(R1.L, R2.L);\n\t" /* pack x16 and y16 */
00129    "[P2] = R2;\n\t"
00130                
00131    /* Samples 1 to ord-1 (using memory) */
00132    "R0 += -1;\n\t"
00133    "R3 = 0;\n\t"
00134    "LC0 = R0;\n\t"
00135    "LOOP filter_start%= LC0;\n\t"
00136    "LOOP_BEGIN filter_start%=;\n\t"
00137       "R3 += 1;\n\t"
00138       "LC1 = R3;\n\t"
00139       
00140       "R1 = [P4++];\n\t"
00141       "A1 = R1;\n\t"
00142       "A0 = 0;\n\t"
00143       "I0 = B0;\n\t"
00144       "I2 = P2;\n\t"
00145       "P2 += 4;\n\t"
00146       "R4 = [I0++] || R5 = [I2--];\n\t"
00147       "LOOP filter_start_inner%= LC1;\n\t"
00148       "LOOP_BEGIN filter_start_inner%=;\n\t"
00149          "A1 -= R4.H*R5.H, A0 += R4.L*R5.L (IS) || R4 = [I0++] || R5 = [I2--];\n\t"
00150       "LOOP_END filter_start_inner%=;\n\t"
00151       "A0 += A1;\n\t"
00152       "R4 = A0;\n\t"
00153       "R4 <<= 3;\n\t" /* shift mem */
00154       "R4.L = R4 (RND);\n\t"
00155       "R2 = W[P0++];\n\t" /* load x */
00156       "R4.L = R4.L + R2.L;\n\t"
00157       "W[P1++] = R4;\n\t" /* store y */
00158       //"R4 <<= 2;\n\t"
00159       //"R2 <<= 2;\n\t"
00160       "R2 = PACK(R4.L, R2.L);\n\t" /* pack x16 and y16 */
00161       "[P2] = R2;\n\t"
00162 
00163    "LOOP_END filter_start%=;\n\t"
00164 
00165    /* Samples ord to N*/   
00166    "R0 = %5;\n\t"
00167    "R0 <<= 1;\n\t"
00168    "I0 = B0;\n\t" /* numden */
00169    "R0 <<= 1;\n\t"   
00170    "L0 = R0;\n\t"
00171    
00172    "R0 = %5;\n\t" /* org */
00173    "R2 = %4;\n\t" /* N */
00174    "R2 = R2 - R0;\n\t"
00175    "R4 = [I0++];\n\t" /* numden */
00176    "LC0 = R2;\n\t"
00177    "P3 = R0;\n\t"
00178    "R0 <<= 2;\n\t"
00179    "R0 += 8;\n\t"
00180    "I2 = P2;\n\t"
00181    "M0 = R0;\n\t"
00182    "A1 = A0 = 0;\n\t"
00183    "R5 = [I2--];\n\t" /* load xy */
00184    "LOOP filter_mid%= LC0;\n\t"
00185    "LOOP_BEGIN filter_mid%=;\n\t"
00186       "LOOP filter_mid_inner%= LC1=P3;\n\t"
00187       "LOOP_BEGIN filter_mid_inner%=;\n\t"
00188          "A1 -= R4.H*R5.H, A0 += R4.L*R5.L (IS) || R4 = [I0++] || R5 = [I2--];\n\t"
00189       "LOOP_END filter_mid_inner%=;\n\t"
00190       "R0 = (A0 += A1) || I2 += M0;\n\t"
00191       "R0 = R0 << 3 || R5 = W[P0++];\n\t" /* load x */
00192       "R0.L = R0 (RND);\n\t"
00193       "R0.L = R0.L + R5.L;\n\t"
00194       "R5 = PACK(R0.L, R5.L) || W[P1++] = R0;\n\t" /* shift y | store y */
00195       "A1 = A0 = 0 || [I2--] = R5\n\t"
00196       "LOOP_END filter_mid%=;\n\t"
00197    "I2 += 4;\n\t"
00198    "P2 = I2;\n\t"
00199    /* Update memory */
00200    "P4 = %6;\n\t"
00201    "R0 = %5;\n\t"
00202    "LC0 = R0;\n\t"
00203    "P0 = B0;\n\t"
00204    "A1 = A0 = 0;\n\t"
00205    "LOOP mem_update%= LC0;\n\t"
00206    "LOOP_BEGIN mem_update%=;\n\t"
00207       "I2 = P2;\n\t"
00208       "I0 = P0;\n\t"
00209       "P0 += 4;\n\t"
00210       "R0 = LC0;\n\t"
00211       "LC1 = R0;\n\t"
00212       "R5 = [I2--] || R4 = [I0++];\n\t"
00213       "LOOP mem_accum%= LC1;\n\t"
00214       "LOOP_BEGIN mem_accum%=;\n\t"
00215          "A1 -= R4.H*R5.H, A0 += R4.L*R5.L (IS) || R4 = [I0++] || R5 = [I2--];\n\t"
00216       "LOOP_END mem_accum%=;\n\t"
00217       "R0 = (A0 += A1);\n\t"
00218       "A1 = A0 = 0 || [P4++] = R0;\n\t"
00219    "LOOP_END mem_update%=;\n\t"
00220    "L0 = 0;\n\t"
00221    : : "m" (xy), "m" (_x), "m" (_y), "m" (numden), "m" (N), "m" (ord), "m" (mem)
00222    : "A0", "A1", "R0", "R1", "R2", "R3", "R4", "R5", "P0", "P1", "P2", "P3", "P4", "B0", "I0", "I2", "L0", "L2", "M0", "memory"
00223    );
00224 
00225 }
00226 
00227 
00228 
00229 #define OVERRIDE_IIR_MEM16
00230 void iir_mem16(const spx_word16_t *_x, const spx_coef_t *den, spx_word16_t *_y, int N, int ord, spx_mem_t *mem, char *stack)
00231 {
00232    VARDECL(spx_word16_t *y);
00233    spx_word16_t *yy;
00234 
00235    ALLOC(y, (N+2), spx_word16_t);
00236    yy = y+2;
00237 
00238    __asm__ __volatile__
00239    (
00240    /* Register setup */
00241    "R0 = %5;\n\t"      /*ord */
00242    
00243    "P1 = %3;\n\t"
00244    "I1 = P1;\n\t"
00245    "B1 = P1;\n\t"
00246    "L1 = 0;\n\t"
00247    
00248    "P3 = %0;\n\t"
00249    "I3 = P3;\n\t"
00250    "L3 = 0;\n\t"
00251    
00252    "P4 = %6;\n\t"
00253    "P0 = %1;\n\t"
00254    "P1 = %2;\n\t"
00255    
00256    /* First sample */
00257    "R1 = [P4++];\n\t"
00258    "R1 = R1 << 3 (S);\n\t"
00259    "R1.L = R1 (RND);\n\t"
00260    "R2 = W[P0++];\n\t"
00261    "R1 = R1 + R2;\n\t"
00262    "W[P1++] = R1;\n\t"
00263    "W[P3] = R1;\n\t"
00264 
00265    /* Samples 1 to ord-1 (using memory) */
00266    "R0 += -1;\n\t"
00267    "R3 = 0;\n\t"
00268    "LC0 = R0;\n\t"
00269    "LOOP filter_start%= LC0;\n\t"
00270    "LOOP_BEGIN filter_start%=;\n\t"
00271       "R3 += 1;\n\t"
00272       "LC1 = R3;\n\t"
00273       
00274       "R1 = [P4++];\n\t"
00275       "A1 = R1;\n\t"
00276       "I1 = B1;\n\t"
00277       "I3 = P3;\n\t"
00278       "P3 += 2;\n\t"
00279       "LOOP filter_start_inner%= LC1;\n\t"
00280       "LOOP_BEGIN filter_start_inner%=;\n\t"
00281          "R4.L = W[I1++];\n\t"
00282          "R5.L = W[I3--];\n\t"
00283          "A1 -= R4.L*R5.L (IS);\n\t"
00284       "LOOP_END filter_start_inner%=;\n\t"
00285    
00286       "R1 = A1;\n\t"
00287       "R1 <<= 3;\n\t"
00288       "R1.L = R1 (RND);\n\t"
00289       "R2 = W[P0++];\n\t"
00290       "R1 = R1 + R2;\n\t"
00291       "W[P1++] = R1;\n\t"
00292       "W[P3] = R1;\n\t"
00293    "LOOP_END filter_start%=;\n\t"
00294 
00295    /* Samples ord to N*/   
00296    "R0 = %5;\n\t"
00297    "R0 <<= 1;\n\t"
00298    "I1 = B1;\n\t"
00299    "L1 = R0;\n\t"
00300    
00301    "R0 = %5;\n\t"
00302    "R2 = %4;\n\t"
00303    "R2 = R2 - R0;\n\t"
00304    "R4.L = W[I1++];\n\t"
00305    "LC0 = R2;\n\t"
00306    "LOOP filter_mid%= LC0;\n\t"
00307    "LOOP_BEGIN filter_mid%=;\n\t"
00308       "LC1 = R0;\n\t"
00309       "A1 = 0;\n\t"
00310       "I3 = P3;\n\t"
00311       "P3 += 2;\n\t"
00312       "R5.L = W[I3--];\n\t"
00313       "LOOP filter_mid_inner%= LC1;\n\t"
00314       "LOOP_BEGIN filter_mid_inner%=;\n\t"
00315          "A1 -= R4.L*R5.L (IS) || R4.L = W[I1++] || R5.L = W[I3--];\n\t"
00316       "LOOP_END filter_mid_inner%=;\n\t"
00317       "R1 = A1;\n\t"
00318       "R1 = R1 << 3 || R2 = W[P0++];\n\t"
00319       "R1.L = R1 (RND);\n\t"
00320       "R1 = R1 + R2;\n\t"
00321       "W[P1++] = R1;\n\t"
00322       "W[P3] = R1;\n\t"
00323    "LOOP_END filter_mid%=;\n\t"
00324      
00325    /* Update memory */
00326    "P4 = %6;\n\t"
00327    "R0 = %5;\n\t"
00328    "LC0 = R0;\n\t"
00329    "P1 = B1;\n\t"
00330    "LOOP mem_update%= LC0;\n\t"
00331    "LOOP_BEGIN mem_update%=;\n\t"
00332       "A0 = 0;\n\t"
00333       "I3 = P3;\n\t"
00334       "I1 = P1;\n\t"
00335       "P1 += 2;\n\t"
00336       "R0 = LC0;\n\t"
00337       "LC1=R0;\n\t"
00338       "R5.L = W[I3--] || R4.L = W[I1++];\n\t"
00339       "LOOP mem_accum%= LC1;\n\t"
00340       "LOOP_BEGIN mem_accum%=;\n\t"
00341          "A0 -= R4.L*R5.L (IS) || R4.L = W[I1++] || R5.L = W[I3--];\n\t"
00342       "LOOP_END mem_accum%=;\n\t"
00343       "R0 = A0;\n\t"
00344       "[P4++] = R0;\n\t"
00345    "LOOP_END mem_update%=;\n\t"
00346    "L1 = 0;\n\t"
00347    : : "m" (yy), "m" (_x), "m" (_y), "m" (den), "m" (N), "m" (ord), "m" (mem)
00348    : "A0", "A1", "R0", "R1", "R2", "R3", "R4", "R5", "P0", "P1", "P2", "P3", "P4", "B1", "I1", "I3", "L1", "L3", "memory"
00349    );
00350 
00351 }
00352 
00353 
00354 #define OVERRIDE_FIR_MEM16
00355 void fir_mem16(const spx_word16_t *x, const spx_coef_t *num, spx_word16_t *y, int N, int ord, spx_mem_t *mem, char *stack)
00356 {
00357    int i;
00358    spx_coef_t den2[12];
00359    spx_coef_t *den;
00360    den = (spx_coef_t*)((((int)den2)+4)&0xfffffffc);
00361    for (i=0;i<10;i++)
00362       den[i] = 0;
00363    filter_mem16(x, num, den, y, N, ord, mem, stack);
00364 }
00365 
00366 
00367 #define OVERRIDE_COMPUTE_IMPULSE_RESPONSE
00368 void compute_impulse_response(const spx_coef_t *ak, const spx_coef_t *awk1, const spx_coef_t *awk2, spx_word16_t *y, int N, int ord, char *stack)
00369 {
00370    int i;
00371    VARDECL(spx_word16_t *ytmp);
00372    ALLOC(ytmp, N, spx_word16_t);
00373    spx_word16_t *ytmp2 = ytmp;
00374    y[0] = LPC_SCALING;
00375    for (i=0;i<ord;i++)
00376       y[i+1] = awk1[i];
00377    i++;
00378    for (;i<N;i++)
00379       y[i] = 0;
00380 
00381    N-=1;
00382    __asm__ __volatile__
00383    (
00384          "I0 = %0;\n\t"
00385          "I1 = %1;\n\t"
00386          "L0 = 0;\n\t"
00387          "L1 = 0;\n\t"
00388          "L2 = 0;\n\t"
00389          "L3 = 0;\n\t"
00390          "R0 = 1;\n\t"
00391          "R0 <<= 13;\n\t"
00392          "W[I0] = R0.L;\n\t"
00393          "R0 <<= 1;\n\t"
00394          "W[I1] = R0.L;\n\t"
00395          "R0 = %5;\n\t"
00396          "LC0 = R0;\n\t"
00397          "R2 = 0;\n\t"
00398          "LOOP samples%= LC0;\n\t"
00399          "LOOP_BEGIN samples%=;\n\t"
00400             "R2 += 1;\n\t"
00401             "R2 = MIN(R2, %4);\n\t"
00402             "I0 = %0;\n\t"
00403             "I1 = %1;\n\t"
00404             "I2 = %2;\n\t"
00405             "I3 = %3;\n\t"
00406             "%0 += 2;\n\t"
00407             "%1 += 2;\n\t"
00408             "A1 = A0 = 0;\n\t"
00409             "R0.L = W[I0--] || R1.L = W[I2++];\n\t"
00410             "LC1 = R2;\n\t"
00411             "LOOP filter%= LC1;\n\t"
00412             "LOOP_BEGIN filter%=;\n\t"
00413                "A0 -= R0.L*R1.L (IS) || R0.L = W[I1--] || R1.L = W[I3++];\n\t"
00414                "A1 -= R0.L*R1.L (IS) || R0.L = W[I0--] || R1.L = W[I2++];\n\t"
00415             "LOOP_END filter%=;\n\t"
00416             "R0 = A0, R1 = A1;\n\t"
00417             "R3 = W[%1] (X);\n\t"
00418             "R3 <<= 13;\n\t"
00419             "R0 = R0 + R3;\n\t"
00420             "R3 = R0 >>> 13;\n\t"
00421             "W[%0] = R3.L;\n\t"
00422             "R0 <<= 1;\n\t"
00423             "R1 = R1 + R0;\n\t"
00424             "R1 >>>= 13;\n\t"
00425             "W[%1] = R1.L;\n\t"
00426          "LOOP_END samples%=;\n\t"
00427    : "=a" (ytmp2), "=a" (y)
00428    : "a" (awk2), "a" (ak), "d" (ord), "m" (N), "0" (ytmp2), "1" (y)
00429    : "A0", "A1", "R0", "R1", "R2", "R3", "I0", "I1", "I2", "I3", "L0", "L1", "L2", "L3", "A0", "A1"
00430    );
00431 }
00432 
00433 
00434 
00435 #if 0 /* Equivalent C function for filter_mem2 and compute_impulse_response */
00436 #define min(a,b) ((a)<(b) ? (a):(b))
00437 
00438 void compute_impulse_response(const spx_coef_t *ak, const spx_coef_t *awk1, const spx_coef_t *awk2, spx_word16_t *y, int N, int ord, char *stack)
00439 {
00440    int i,j;
00441    VARDECL(spx_word16_t *ytmp);
00442    ALLOC(ytmp, N, spx_word16_t);
00443    
00444    y[0] = LPC_SCALING;
00445    for (i=0;i<ord;i++)
00446       y[i+1] = awk1[i];
00447    i++;
00448    for (;i<N;i++)
00449       y[i] = 0;
00450 
00451    for (i=0;i<N;i++)
00452    {
00453       spx_word32_t yi = SHL32(EXTEND32(y[i]),LPC_SHIFT);
00454       spx_word32_t yi2 = 0;
00455       for (j=0;j<min(i,ord);j++)
00456       {
00457          yi = MAC16_16(yi, awk2[j], -ytmp[i-j-1]);
00458          yi2 = MAC16_16(yi2, ak[j], -y[i-j-1]);
00459       }
00460       ytmp[i] = EXTRACT16(SHR32(yi,LPC_SHIFT));
00461       yi2 = ADD32(yi2,SHL32(yi,1));
00462       y[i] = EXTRACT16(SHR32(yi2,LPC_SHIFT));
00463    }
00464 
00465 }
00466 
00467 
00468 void filter_mem2(const spx_sig_t *_x, const spx_coef_t *num, const spx_coef_t *den, spx_sig_t *_y, int N, int ord, spx_mem_t *mem)
00469 {
00470    int i,j;
00471    spx_word16_t xi,yi,nyi;
00472    spx_word16_t x[N],y[N];
00473    spx_word16_t *xx, *yy;
00474    xx = x;
00475    yy = y;
00476    for (i=0;i<N;i++)
00477    {
00478       x[i] = EXTRACT16(SHR32(_x[i],SIG_SHIFT));
00479    }
00480    
00481    for (i=0;i<ord;i++)
00482    {
00483       spx_word32_t yi = mem[i];
00484       for (j=0;j<i;j++)
00485       {
00486          yi = MAC16_16(yi, num[j], x[i-j-1]);
00487          yi = MAC16_16(yi, den[j], -y[i-j-1]);
00488       }
00489       _y[i] = ADD32(_x[i],SHL32(yi,1));
00490       y[i] = EXTRACT16(SHR32(_y[i],SIG_SHIFT));
00491    }
00492    for (i=ord;i<N;i++)
00493    {
00494       spx_word32_t yi = 0;
00495       for (j=0;j<ord;j++)
00496       {
00497          yi = MAC16_16(yi, num[j], x[i-j-1]);
00498          yi = MAC16_16(yi, den[j], -y[i-j-1]);
00499       }
00500       _y[i] = ADD32(_x[i],SHL32(yi,1));
00501       y[i] = EXTRACT16(SHR32(_y[i],SIG_SHIFT));
00502    }
00503 
00504    for (i=0;i<ord;i++)
00505    {
00506       spx_mem_t m = 0;
00507       for (j=0;j<ord-i;j++)
00508       {
00509          m = MAC16_16(m, x[N-1-j], num[j+i]);
00510          m = MAC16_16(m, -y[N-1-j], den[j+i]);
00511       }
00512       mem[i] = m;
00513    }
00514 }
00515 #endif