src/lib/dl/ext/speex/filters_bfin.h

   1 /* Copyright (C) 2005 Analog Devices */
   2 /**
   3    @file filters_bfin.h
   4    @brief Various analysis/synthesis filters (Blackfin version)
   5 */
   6 /*
   7    Redistribution and use in source and binary forms, with or without
   8    modification, are permitted provided that the following conditions
   9    are met:
  10
  11    - Redistributions of source code must retain the above copyright
  12    notice, this list of conditions and the following disclaimer.
  13
  14    - Redistributions in binary form must reproduce the above copyright
  15    notice, this list of conditions and the following disclaimer in the
  16    documentation and/or other materials provided with the distribution.
  17
  18    - Neither the name of the Xiph.org Foundation nor the names of its
  19    contributors may be used to endorse or promote products derived from
  20    this software without specific prior written permission.
  21
  22    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  23    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  24    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
  25    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
  26    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
  27    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
  28    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
  29    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
  30    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
  31    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  32    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  33 */
  34
  35 #define OVERRIDE_NORMALIZE16
  36 int normalize16(const spx_sig_t *x, spx_word16_t *y, spx_sig_t max_scale, int len)
  37 {
  38    spx_sig_t max_val=1;
  39    int sig_shift;
  40    __asm__
  41    (
  42    "%0 = 0;\n\t"
  43    "I0 = %1;\n\t"
  44    "L0 = 0;\n\t"
  45    "R1 = [I0++];\n\t"
  46    "LOOP norm_max%= LC0 = %2;\n\t"
  47    "LOOP_BEGIN norm_max%=;\n\t"
  48       "R2 = ABS R1 || R1 = [I0++];\n\t"
  49       "%0 = MAX(%0, R2);\n\t"
  50    "LOOP_END norm_max%=;\n\t"
  51    : "=&d" (max_val)
  52    : "a" (x), "a" (len)
  53    : "R1", "R2"
  54    );
  55
  56    sig_shift=0;
  57    while (max_val>max_scale)
  58    {
  59       sig_shift++;
  60       max_val >>= 1;
  61    }
  62
  63    __asm__ __volatile__
  64    (
  65    "I0 = %0;\n\t"
  66    "L0 = 0;\n\t"
  67    "P1 = %1;\n\t"
  68    "R0 = [I0++];\n\t"
  69    "LOOP norm_shift%= LC0 = %3;\n\t"
  70    "LOOP_BEGIN norm_shift%=;\n\t"
  71       "R1 = ASHIFT R0 by %2.L || R0 = [I0++];\n\t"
  72       "W[P1++] = R1;\n\t"
  73    "LOOP_END norm_shift%=;\n\t"
  74    "R1 = ASHIFT R0 by %2.L;\n\t"
  75    "W[P1++] = R1;\n\t"
  76    : : "a" (x), "a" (y), "d" (-sig_shift), "a" (len-1)
  77    : "I0", "L0", "P1", "R0", "R1", "memory"
  78    );
  79    return sig_shift;
  80 }
  81
  82
  83
  84 #define OVERRIDE_FILTER_MEM16
  85 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)
  86 {
  87    VARDECL(spx_word32_t *xy2);
  88    VARDECL(spx_word32_t *numden_a);
  89    spx_word32_t *xy;
  90    spx_word16_t *numden;
  91    int i;
  92
  93    ALLOC(xy2, (N+1), spx_word32_t);
  94    ALLOC(numden_a, (2*ord+2), spx_word32_t);
  95    xy = xy2+1;
  96    numden = (spx_word16_t*) numden_a;
  97
  98    for (i=0;i<ord;i++)
  99    {
 100       numden[2*i] = num[i];
 101       numden[2*i+1] = den[i];
 102    }
 103    __asm__ __volatile__
 104    (
 105    /* Register setup */
 106    "R0 = %5;\n\t"      /*ord */
 107
 108    "P0 = %3;\n\t"
 109    "I0 = P0;\n\t"
 110    "B0 = P0;\n\t" /* numden */
 111    "L0 = 0;\n\t"
 112
 113    "P2 = %0;\n\t" /* Fused xy */
 114    "I2 = P2;\n\t"
 115    "L2 = 0;\n\t"
 116
 117    "P4 = %6;\n\t" /* mem */
 118    "P0 = %1;\n\t" /* _x */
 119    "P1 = %2;\n\t" /* _y */
 120
 121    /* First sample */
 122    "R1 = [P4++];\n\t"
 123    "R1 <<= 3;\n\t" /* shift mem */
 124    "R1.L = R1 (RND);\n\t"
 125    "R2 = W[P0++];\n\t" /* load x[0] */
 126    "R1.L = R1.L + R2.L;\n\t"
 127    "W[P1++] = R1;\n\t" /* store y[0] */
 128    "R2 = PACK(R1.L, R2.L);\n\t" /* pack x16 and y16 */
 129    "[P2] = R2;\n\t"
 130
 131    /* Samples 1 to ord-1 (using memory) */
 132    "R0 += -1;\n\t"
 133    "R3 = 0;\n\t"
 134    "LC0 = R0;\n\t"
 135    "LOOP filter_start%= LC0;\n\t"
 136    "LOOP_BEGIN filter_start%=;\n\t"
 137       "R3 += 1;\n\t"
 138       "LC1 = R3;\n\t"
 139
 140       "R1 = [P4++];\n\t"
 141       "A1 = R1;\n\t"
 142       "A0 = 0;\n\t"
 143       "I0 = B0;\n\t"
 144       "I2 = P2;\n\t"
 145       "P2 += 4;\n\t"
 146       "R4 = [I0++] || R5 = [I2--];\n\t"
 147       "LOOP filter_start_inner%= LC1;\n\t"
 148       "LOOP_BEGIN filter_start_inner%=;\n\t"
 149          "A1 -= R4.H*R5.H, A0 += R4.L*R5.L (IS) || R4 = [I0++] || R5 = [I2--];\n\t"
 150       "LOOP_END filter_start_inner%=;\n\t"
 151       "A0 += A1;\n\t"
 152       "R4 = A0;\n\t"
 153       "R4 <<= 3;\n\t" /* shift mem */
 154       "R4.L = R4 (RND);\n\t"
 155       "R2 = W[P0++];\n\t" /* load x */
 156       "R4.L = R4.L + R2.L;\n\t"
 157       "W[P1++] = R4;\n\t" /* store y */
 158       //"R4 <<= 2;\n\t"
 159       //"R2 <<= 2;\n\t"
 160       "R2 = PACK(R4.L, R2.L);\n\t" /* pack x16 and y16 */
 161       "[P2] = R2;\n\t"
 162
 163    "LOOP_END filter_start%=;\n\t"
 164
 165    /* Samples ord to N*/
 166    "R0 = %5;\n\t"
 167    "R0 <<= 1;\n\t"
 168    "I0 = B0;\n\t" /* numden */
 169    "R0 <<= 1;\n\t"
 170    "L0 = R0;\n\t"
 171
 172    "R0 = %5;\n\t" /* org */
 173    "R2 = %4;\n\t" /* N */
 174    "R2 = R2 - R0;\n\t"
 175    "R4 = [I0++];\n\t" /* numden */
 176    "LC0 = R2;\n\t"
 177    "P3 = R0;\n\t"
 178    "R0 <<= 2;\n\t"
 179    "R0 += 8;\n\t"
 180    "I2 = P2;\n\t"
 181    "M0 = R0;\n\t"
 182    "A1 = A0 = 0;\n\t"
 183    "R5 = [I2--];\n\t" /* load xy */
 184    "LOOP filter_mid%= LC0;\n\t"
 185    "LOOP_BEGIN filter_mid%=;\n\t"
 186       "LOOP filter_mid_inner%= LC1=P3;\n\t"
 187       "LOOP_BEGIN filter_mid_inner%=;\n\t"
 188          "A1 -= R4.H*R5.H, A0 += R4.L*R5.L (IS) || R4 = [I0++] || R5 = [I2--];\n\t"
 189       "LOOP_END filter_mid_inner%=;\n\t"
 190       "R0 = (A0 += A1) || I2 += M0;\n\t"
 191       "R0 = R0 << 3 || R5 = W[P0++];\n\t" /* load x */
 192       "R0.L = R0 (RND);\n\t"
 193       "R0.L = R0.L + R5.L;\n\t"
 194       "R5 = PACK(R0.L, R5.L) || W[P1++] = R0;\n\t" /* shift y | store y */
 195       "A1 = A0 = 0 || [I2--] = R5\n\t"
 196       "LOOP_END filter_mid%=;\n\t"
 197    "I2 += 4;\n\t"
 198    "P2 = I2;\n\t"
 199    /* Update memory */
 200    "P4 = %6;\n\t"
 201    "R0 = %5;\n\t"
 202    "LC0 = R0;\n\t"
 203    "P0 = B0;\n\t"
 204    "A1 = A0 = 0;\n\t"
 205    "LOOP mem_update%= LC0;\n\t"
 206    "LOOP_BEGIN mem_update%=;\n\t"
 207       "I2 = P2;\n\t"
 208       "I0 = P0;\n\t"
 209       "P0 += 4;\n\t"
 210       "R0 = LC0;\n\t"
 211       "LC1 = R0;\n\t"
 212       "R5 = [I2--] || R4 = [I0++];\n\t"
 213       "LOOP mem_accum%= LC1;\n\t"
 214       "LOOP_BEGIN mem_accum%=;\n\t"
 215          "A1 -= R4.H*R5.H, A0 += R4.L*R5.L (IS) || R4 = [I0++] || R5 = [I2--];\n\t"
 216       "LOOP_END mem_accum%=;\n\t"
 217       "R0 = (A0 += A1);\n\t"
 218       "A1 = A0 = 0 || [P4++] = R0;\n\t"
 219    "LOOP_END mem_update%=;\n\t"
 220    "L0 = 0;\n\t"
 221    : : "m" (xy), "m" (_x), "m" (_y), "m" (numden), "m" (N), "m" (ord), "m" (mem)
 222    : "A0", "A1", "R0", "R1", "R2", "R3", "R4", "R5", "P0", "P1", "P2", "P3", "P4", "B0", "I0", "I2", "L0", "L2", "M0", "memory"
 223    );
 224
 225 }
 226
 227
 228
 229 #define OVERRIDE_IIR_MEM16
 230 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)
 231 {
 232    VARDECL(spx_word16_t *y);
 233    spx_word16_t *yy;
 234
 235    ALLOC(y, (N+2), spx_word16_t);
 236    yy = y+2;
 237
 238    __asm__ __volatile__
 239    (
 240    /* Register setup */
 241    "R0 = %5;\n\t"      /*ord */
 242
 243    "P1 = %3;\n\t"
 244    "I1 = P1;\n\t"
 245    "B1 = P1;\n\t"
 246    "L1 = 0;\n\t"
 247
 248    "P3 = %0;\n\t"
 249    "I3 = P3;\n\t"
 250    "L3 = 0;\n\t"
 251
 252    "P4 = %6;\n\t"
 253    "P0 = %1;\n\t"
 254    "P1 = %2;\n\t"
 255
 256    /* First sample */
 257    "R1 = [P4++];\n\t"
 258    "R1 = R1 << 3 (S);\n\t"
 259    "R1.L = R1 (RND);\n\t"
 260    "R2 = W[P0++];\n\t"
 261    "R1 = R1 + R2;\n\t"
 262    "W[P1++] = R1;\n\t"
 263    "W[P3] = R1;\n\t"
 264
 265    /* Samples 1 to ord-1 (using memory) */
 266    "R0 += -1;\n\t"
 267    "R3 = 0;\n\t"
 268    "LC0 = R0;\n\t"
 269    "LOOP filter_start%= LC0;\n\t"
 270    "LOOP_BEGIN filter_start%=;\n\t"
 271       "R3 += 1;\n\t"
 272       "LC1 = R3;\n\t"
 273
 274       "R1 = [P4++];\n\t"
 275       "A1 = R1;\n\t"
 276       "I1 = B1;\n\t"
 277       "I3 = P3;\n\t"
 278       "P3 += 2;\n\t"
 279       "LOOP filter_start_inner%= LC1;\n\t"
 280       "LOOP_BEGIN filter_start_inner%=;\n\t"
 281          "R4.L = W[I1++];\n\t"
 282          "R5.L = W[I3--];\n\t"
 283          "A1 -= R4.L*R5.L (IS);\n\t"
 284       "LOOP_END filter_start_inner%=;\n\t"
 285
 286       "R1 = A1;\n\t"
 287       "R1 <<= 3;\n\t"
 288       "R1.L = R1 (RND);\n\t"
 289       "R2 = W[P0++];\n\t"
 290       "R1 = R1 + R2;\n\t"
 291       "W[P1++] = R1;\n\t"
 292       "W[P3] = R1;\n\t"
 293    "LOOP_END filter_start%=;\n\t"
 294
 295    /* Samples ord to N*/
 296    "R0 = %5;\n\t"
 297    "R0 <<= 1;\n\t"
 298    "I1 = B1;\n\t"
 299    "L1 = R0;\n\t"
 300
 301    "R0 = %5;\n\t"
 302    "R2 = %4;\n\t"
 303    "R2 = R2 - R0;\n\t"
 304    "R4.L = W[I1++];\n\t"
 305    "LC0 = R2;\n\t"
 306    "LOOP filter_mid%= LC0;\n\t"
 307    "LOOP_BEGIN filter_mid%=;\n\t"
 308       "LC1 = R0;\n\t"
 309       "A1 = 0;\n\t"
 310       "I3 = P3;\n\t"
 311       "P3 += 2;\n\t"
 312       "R5.L = W[I3--];\n\t"
 313       "LOOP filter_mid_inner%= LC1;\n\t"
 314       "LOOP_BEGIN filter_mid_inner%=;\n\t"
 315          "A1 -= R4.L*R5.L (IS) || R4.L = W[I1++] || R5.L = W[I3--];\n\t"
 316       "LOOP_END filter_mid_inner%=;\n\t"
 317       "R1 = A1;\n\t"
 318       "R1 = R1 << 3 || R2 = W[P0++];\n\t"
 319       "R1.L = R1 (RND);\n\t"
 320       "R1 = R1 + R2;\n\t"
 321       "W[P1++] = R1;\n\t"
 322       "W[P3] = R1;\n\t"
 323    "LOOP_END filter_mid%=;\n\t"
 324
 325    /* Update memory */
 326    "P4 = %6;\n\t"
 327    "R0 = %5;\n\t"
 328    "LC0 = R0;\n\t"
 329    "P1 = B1;\n\t"
 330    "LOOP mem_update%= LC0;\n\t"
 331    "LOOP_BEGIN mem_update%=;\n\t"
 332       "A0 = 0;\n\t"
 333       "I3 = P3;\n\t"
 334       "I1 = P1;\n\t"
 335       "P1 += 2;\n\t"
 336       "R0 = LC0;\n\t"
 337       "LC1=R0;\n\t"
 338       "R5.L = W[I3--] || R4.L = W[I1++];\n\t"
 339       "LOOP mem_accum%= LC1;\n\t"
 340       "LOOP_BEGIN mem_accum%=;\n\t"
 341          "A0 -= R4.L*R5.L (IS) || R4.L = W[I1++] || R5.L = W[I3--];\n\t"
 342       "LOOP_END mem_accum%=;\n\t"
 343       "R0 = A0;\n\t"
 344       "[P4++] = R0;\n\t"
 345    "LOOP_END mem_update%=;\n\t"
 346    "L1 = 0;\n\t"
 347    : : "m" (yy), "m" (_x), "m" (_y), "m" (den), "m" (N), "m" (ord), "m" (mem)
 348    : "A0", "A1", "R0", "R1", "R2", "R3", "R4", "R5", "P0", "P1", "P2", "P3", "P4", "B1", "I1", "I3", "L1", "L3", "memory"
 349    );
 350
 351 }
 352
 353
 354 #define OVERRIDE_FIR_MEM16
 355 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)
 356 {
 357    int i;
 358    spx_coef_t den2[12];
 359    spx_coef_t *den;
 360    den = (spx_coef_t*)((((int)den2)+4)&0xfffffffc);
 361    for (i=0;i<10;i++)
 362       den[i] = 0;
 363    filter_mem16(x, num, den, y, N, ord, mem, stack);
 364 }
 365
 366
 367 #define OVERRIDE_COMPUTE_IMPULSE_RESPONSE
 368 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)
 369 {
 370    int i;
 371    VARDECL(spx_word16_t *ytmp);
 372    ALLOC(ytmp, N, spx_word16_t);
 373    spx_word16_t *ytmp2 = ytmp;
 374    y[0] = LPC_SCALING;
 375    for (i=0;i<ord;i++)
 376       y[i+1] = awk1[i];
 377    i++;
 378    for (;i<N;i++)
 379       y[i] = 0;
 380
 381    N-=1;
 382    __asm__ __volatile__
 383    (
 384          "I0 = %0;\n\t"
 385          "I1 = %1;\n\t"
 386          "L0 = 0;\n\t"
 387          "L1 = 0;\n\t"
 388          "L2 = 0;\n\t"
 389          "L3 = 0;\n\t"
 390          "R0 = 1;\n\t"
 391          "R0 <<= 13;\n\t"
 392          "W[I0] = R0.L;\n\t"
 393          "R0 <<= 1;\n\t"
 394          "W[I1] = R0.L;\n\t"
 395          "R0 = %5;\n\t"
 396          "LC0 = R0;\n\t"
 397          "R2 = 0;\n\t"
 398          "LOOP samples%= LC0;\n\t"
 399          "LOOP_BEGIN samples%=;\n\t"
 400             "R2 += 1;\n\t"
 401             "R2 = MIN(R2, %4);\n\t"
 402             "I0 = %0;\n\t"
 403             "I1 = %1;\n\t"
 404             "I2 = %2;\n\t"
 405             "I3 = %3;\n\t"
 406             "%0 += 2;\n\t"
 407             "%1 += 2;\n\t"
 408             "A1 = A0 = 0;\n\t"
 409             "R0.L = W[I0--] || R1.L = W[I2++];\n\t"
 410             "LC1 = R2;\n\t"
 411             "LOOP filter%= LC1;\n\t"
 412             "LOOP_BEGIN filter%=;\n\t"
 413                "A0 -= R0.L*R1.L (IS) || R0.L = W[I1--] || R1.L = W[I3++];\n\t"
 414                "A1 -= R0.L*R1.L (IS) || R0.L = W[I0--] || R1.L = W[I2++];\n\t"
 415             "LOOP_END filter%=;\n\t"
 416             "R0 = A0, R1 = A1;\n\t"
 417             "R3 = W[%1] (X);\n\t"
 418             "R3 <<= 13;\n\t"
 419             "R0 = R0 + R3;\n\t"
 420             "R3 = R0 >>> 13;\n\t"
 421             "W[%0] = R3.L;\n\t"
 422             "R0 <<= 1;\n\t"
 423             "R1 = R1 + R0;\n\t"
 424             "R1 >>>= 13;\n\t"
 425             "W[%1] = R1.L;\n\t"
 426          "LOOP_END samples%=;\n\t"
 427    : "=a" (ytmp2), "=a" (y)
 428    : "a" (awk2), "a" (ak), "d" (ord), "m" (N), "0" (ytmp2), "1" (y)
 429    : "A0", "A1", "R0", "R1", "R2", "R3", "I0", "I1", "I2", "I3", "L0", "L1", "L2", "L3", "A0", "A1"
 430    );
 431 }
 432
 433
 434
 435 #if 0 /* Equivalent C function for filter_mem2 and compute_impulse_response */
 436 #define min(a,b) ((a)<(b) ? (a):(b))
 437
 438 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)
 439 {
 440    int i,j;
 441    VARDECL(spx_word16_t *ytmp);
 442    ALLOC(ytmp, N, spx_word16_t);
 443
 444    y[0] = LPC_SCALING;
 445    for (i=0;i<ord;i++)
 446       y[i+1] = awk1[i];
 447    i++;
 448    for (;i<N;i++)
 449       y[i] = 0;
 450
 451    for (i=0;i<N;i++)
 452    {
 453       spx_word32_t yi = SHL32(EXTEND32(y[i]),LPC_SHIFT);
 454       spx_word32_t yi2 = 0;
 455       for (j=0;j<min(i,ord);j++)
 456       {
 457          yi = MAC16_16(yi, awk2[j], -ytmp[i-j-1]);
 458          yi2 = MAC16_16(yi2, ak[j], -y[i-j-1]);
 459       }
 460       ytmp[i] = EXTRACT16(SHR32(yi,LPC_SHIFT));
 461       yi2 = ADD32(yi2,SHL32(yi,1));
 462       y[i] = EXTRACT16(SHR32(yi2,LPC_SHIFT));
 463    }
 464
 465 }
 466
 467
 468 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)
 469 {
 470    int i,j;
 471    spx_word16_t xi,yi,nyi;
 472    spx_word16_t x[N],y[N];
 473    spx_word16_t *xx, *yy;
 474    xx = x;
 475    yy = y;
 476    for (i=0;i<N;i++)
 477    {
 478       x[i] = EXTRACT16(SHR32(_x[i],SIG_SHIFT));
 479    }
 480
 481    for (i=0;i<ord;i++)
 482    {
 483       spx_word32_t yi = mem[i];
 484       for (j=0;j<i;j++)
 485       {
 486          yi = MAC16_16(yi, num[j], x[i-j-1]);
 487          yi = MAC16_16(yi, den[j], -y[i-j-1]);
 488       }
 489       _y[i] = ADD32(_x[i],SHL32(yi,1));
 490       y[i] = EXTRACT16(SHR32(_y[i],SIG_SHIFT));
 491    }
 492    for (i=ord;i<N;i++)
 493    {
 494       spx_word32_t yi = 0;
 495       for (j=0;j<ord;j++)
 496       {
 497          yi = MAC16_16(yi, num[j], x[i-j-1]);
 498          yi = MAC16_16(yi, den[j], -y[i-j-1]);
 499       }
 500       _y[i] = ADD32(_x[i],SHL32(yi,1));
 501       y[i] = EXTRACT16(SHR32(_y[i],SIG_SHIFT));
 502    }
 503
 504    for (i=0;i<ord;i++)
 505    {
 506       spx_mem_t m = 0;
 507       for (j=0;j<ord-i;j++)
 508       {
 509          m = MAC16_16(m, x[N-1-j], num[j+i]);
 510          m = MAC16_16(m, -y[N-1-j], den[j+i]);
 511       }
 512       mem[i] = m;
 513    }
 514 }
 515 #endif