| /* |
| ** Copyright 2003-2010, VisualOn, Inc. |
| ** |
| ** Licensed under the Apache License, Version 2.0 (the "License"); |
| ** you may not use this file except in compliance with the License. |
| ** You may obtain a copy of the License at |
| ** |
| ** http://www.apache.org/licenses/LICENSE-2.0 |
| ** |
| ** Unless required by applicable law or agreed to in writing, software |
| ** distributed under the License is distributed on an "AS IS" BASIS, |
| ** WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| ** See the License for the specific language governing permissions and |
| ** limitations under the License. |
| */ |
| |
| /*********************************************************************** |
| * File: c4t64fx.c * |
| * * |
| * Description:Performs algebraic codebook search for higher modes * |
| * * |
| ************************************************************************/ |
| |
| /************************************************************************ |
| * Function: ACELP_4t64_fx() * |
| * * |
| * 20, 36, 44, 52, 64, 72, 88 bits algebraic codebook. * |
| * 4 tracks x 16 positions per track = 64 samples. * |
| * * |
| * 20 bits --> 4 pulses in a frame of 64 samples. * |
| * 36 bits --> 8 pulses in a frame of 64 samples. * |
| * 44 bits --> 10 pulses in a frame of 64 samples. * |
| * 52 bits --> 12 pulses in a frame of 64 samples. * |
| * 64 bits --> 16 pulses in a frame of 64 samples. * |
| * 72 bits --> 18 pulses in a frame of 64 samples. * |
| * 88 bits --> 24 pulses in a frame of 64 samples. * |
| * * |
| * All pulses can have two (2) possible amplitudes: +1 or -1. * |
| * Each pulse can have sixteen (16) possible positions. * |
| *************************************************************************/ |
| |
| #include "typedef.h" |
| #include "basic_op.h" |
| #include "math_op.h" |
| #include "acelp.h" |
| #include "cnst.h" |
| |
| #include "q_pulse.h" |
| |
| #undef LOG_TAG |
| #define LOG_TAG "amrwbenc" |
| #include "log/log.h" |
| |
| static Word16 tipos[36] = { |
| 0, 1, 2, 3, /* starting point &ipos[0], 1st iter */ |
| 1, 2, 3, 0, /* starting point &ipos[4], 2nd iter */ |
| 2, 3, 0, 1, /* starting point &ipos[8], 3rd iter */ |
| 3, 0, 1, 2, /* starting point &ipos[12], 4th iter */ |
| 0, 1, 2, 3, |
| 1, 2, 3, 0, |
| 2, 3, 0, 1, |
| 3, 0, 1, 2, |
| 0, 1, 2, 3}; /* end point for 24 pulses &ipos[35], 4th iter */ |
| |
| #define NB_PULSE_MAX 24 |
| |
| #define L_SUBFR 64 |
| #define NB_TRACK 4 |
| #define STEP 4 |
| #define NB_POS 16 |
| #define MSIZE 256 |
| #define NB_MAX 8 |
| #define NPMAXPT ((NB_PULSE_MAX+NB_TRACK-1)/NB_TRACK) |
| |
| /* Private functions */ |
| void cor_h_vec_012( |
| Word16 h[], /* (i) scaled impulse response */ |
| Word16 vec[], /* (i) scaled vector (/8) to correlate with h[] */ |
| Word16 track, /* (i) track to use */ |
| Word16 sign[], /* (i) sign vector */ |
| Word16 rrixix[][NB_POS], /* (i) correlation of h[x] with h[x] */ |
| Word16 cor_1[], /* (o) result of correlation (NB_POS elements) */ |
| Word16 cor_2[] /* (o) result of correlation (NB_POS elements) */ |
| ); |
| |
| void cor_h_vec_012_asm( |
| Word16 h[], /* (i) scaled impulse response */ |
| Word16 vec[], /* (i) scaled vector (/8) to correlate with h[] */ |
| Word16 track, /* (i) track to use */ |
| Word16 sign[], /* (i) sign vector */ |
| Word16 rrixix[][NB_POS], /* (i) correlation of h[x] with h[x] */ |
| Word16 cor_1[], /* (o) result of correlation (NB_POS elements) */ |
| Word16 cor_2[] /* (o) result of correlation (NB_POS elements) */ |
| ); |
| |
| void cor_h_vec_30( |
| Word16 h[], /* (i) scaled impulse response */ |
| Word16 vec[], /* (i) scaled vector (/8) to correlate with h[] */ |
| Word16 track, /* (i) track to use */ |
| Word16 sign[], /* (i) sign vector */ |
| Word16 rrixix[][NB_POS], /* (i) correlation of h[x] with h[x] */ |
| Word16 cor_1[], /* (o) result of correlation (NB_POS elements) */ |
| Word16 cor_2[] /* (o) result of correlation (NB_POS elements) */ |
| ); |
| |
| void search_ixiy( |
| Word16 nb_pos_ix, /* (i) nb of pos for pulse 1 (1..8) */ |
| Word16 track_x, /* (i) track of pulse 1 */ |
| Word16 track_y, /* (i) track of pulse 2 */ |
| Word16 * ps, /* (i/o) correlation of all fixed pulses */ |
| Word16 * alp, /* (i/o) energy of all fixed pulses */ |
| Word16 * ix, /* (o) position of pulse 1 */ |
| Word16 * iy, /* (o) position of pulse 2 */ |
| Word16 dn[], /* (i) corr. between target and h[] */ |
| Word16 dn2[], /* (i) vector of selected positions */ |
| Word16 cor_x[], /* (i) corr. of pulse 1 with fixed pulses */ |
| Word16 cor_y[], /* (i) corr. of pulse 2 with fixed pulses */ |
| Word16 rrixiy[][MSIZE] /* (i) corr. of pulse 1 with pulse 2 */ |
| ); |
| |
| |
| void ACELP_4t64_fx( |
| Word16 dn[], /* (i) <12b : correlation between target x[] and H[] */ |
| Word16 cn[], /* (i) <12b : residual after long term prediction */ |
| Word16 H[], /* (i) Q12: impulse response of weighted synthesis filter */ |
| Word16 code[], /* (o) Q9 : algebraic (fixed) codebook excitation */ |
| Word16 y[], /* (o) Q9 : filtered fixed codebook excitation */ |
| Word16 nbbits, /* (i) : 20, 36, 44, 52, 64, 72 or 88 bits */ |
| Word16 ser_size, /* (i) : bit rate */ |
| Word16 _index[] /* (o) : index (20): 5+5+5+5 = 20 bits. */ |
| /* (o) : index (36): 9+9+9+9 = 36 bits. */ |
| /* (o) : index (44): 13+9+13+9 = 44 bits. */ |
| /* (o) : index (52): 13+13+13+13 = 52 bits. */ |
| /* (o) : index (64): 2+2+2+2+14+14+14+14 = 64 bits. */ |
| /* (o) : index (72): 10+2+10+2+10+14+10+14 = 72 bits. */ |
| /* (o) : index (88): 11+11+11+11+11+11+11+11 = 88 bits. */ |
| ) |
| { |
| Word32 i, j, k; |
| Word16 st, ix, iy, pos, index, track, nb_pulse, nbiter, j_temp; |
| Word16 psk, ps, alpk, alp, val, k_cn, k_dn, exp; |
| Word16 *p0, *p1, *p2, *p3, *psign; |
| Word16 *h, *h_inv, *ptr_h1, *ptr_h2, *ptr_hf, h_shift; |
| Word32 s, cor, L_tmp, L_index; |
| Word16 dn2[L_SUBFR], sign[L_SUBFR], vec[L_SUBFR]; |
| Word16 ind[NPMAXPT * NB_TRACK]; |
| Word16 codvec[NB_PULSE_MAX], nbpos[10]; |
| Word16 cor_x[NB_POS], cor_y[NB_POS], pos_max[NB_TRACK]; |
| Word16 h_buf[4 * L_SUBFR]; |
| Word16 rrixix[NB_TRACK][NB_POS], rrixiy[NB_TRACK][MSIZE]; |
| Word16 ipos[NB_PULSE_MAX]; |
| |
| switch (nbbits) |
| { |
| case 20: /* 20 bits, 4 pulses, 4 tracks */ |
| nbiter = 4; /* 4x16x16=1024 loop */ |
| alp = 8192; /* alp = 2.0 (Q12) */ |
| nb_pulse = 4; |
| nbpos[0] = 4; |
| nbpos[1] = 8; |
| break; |
| case 36: /* 36 bits, 8 pulses, 4 tracks */ |
| nbiter = 4; /* 4x20x16=1280 loop */ |
| alp = 4096; /* alp = 1.0 (Q12) */ |
| nb_pulse = 8; |
| nbpos[0] = 4; |
| nbpos[1] = 8; |
| nbpos[2] = 8; |
| break; |
| case 44: /* 44 bits, 10 pulses, 4 tracks */ |
| nbiter = 4; /* 4x26x16=1664 loop */ |
| alp = 4096; /* alp = 1.0 (Q12) */ |
| nb_pulse = 10; |
| nbpos[0] = 4; |
| nbpos[1] = 6; |
| nbpos[2] = 8; |
| nbpos[3] = 8; |
| break; |
| case 52: /* 52 bits, 12 pulses, 4 tracks */ |
| nbiter = 4; /* 4x26x16=1664 loop */ |
| alp = 4096; /* alp = 1.0 (Q12) */ |
| nb_pulse = 12; |
| nbpos[0] = 4; |
| nbpos[1] = 6; |
| nbpos[2] = 8; |
| nbpos[3] = 8; |
| break; |
| case 64: /* 64 bits, 16 pulses, 4 tracks */ |
| nbiter = 3; /* 3x36x16=1728 loop */ |
| alp = 3277; /* alp = 0.8 (Q12) */ |
| nb_pulse = 16; |
| nbpos[0] = 4; |
| nbpos[1] = 4; |
| nbpos[2] = 6; |
| nbpos[3] = 6; |
| nbpos[4] = 8; |
| nbpos[5] = 8; |
| break; |
| case 72: /* 72 bits, 18 pulses, 4 tracks */ |
| nbiter = 3; /* 3x35x16=1680 loop */ |
| alp = 3072; /* alp = 0.75 (Q12) */ |
| nb_pulse = 18; |
| nbpos[0] = 2; |
| nbpos[1] = 3; |
| nbpos[2] = 4; |
| nbpos[3] = 5; |
| nbpos[4] = 6; |
| nbpos[5] = 7; |
| nbpos[6] = 8; |
| break; |
| case 88: /* 88 bits, 24 pulses, 4 tracks */ |
| if(ser_size > 462) |
| nbiter = 1; |
| else |
| nbiter = 2; /* 2x53x16=1696 loop */ |
| |
| alp = 2048; /* alp = 0.5 (Q12) */ |
| nb_pulse = 24; |
| nbpos[0] = 2; |
| nbpos[1] = 2; |
| nbpos[2] = 3; |
| nbpos[3] = 4; |
| nbpos[4] = 5; |
| nbpos[5] = 6; |
| nbpos[6] = 7; |
| nbpos[7] = 8; |
| nbpos[8] = 8; |
| nbpos[9] = 8; |
| break; |
| default: |
| nbiter = 0; |
| alp = 0; |
| nb_pulse = 0; |
| } |
| |
| for (i = 0; i < nb_pulse; i++) |
| { |
| codvec[i] = i; |
| } |
| |
| /*----------------------------------------------------------------* |
| * Find sign for each pulse position. * |
| *----------------------------------------------------------------*/ |
| /* calculate energy for normalization of cn[] and dn[] */ |
| /* set k_cn = 32..32767 (ener_cn = 2^30..256-0) */ |
| #ifdef ASM_OPT /* asm optimization branch */ |
| s = Dot_product12_asm(cn, cn, L_SUBFR, &exp); |
| #else |
| s = Dot_product12(cn, cn, L_SUBFR, &exp); |
| #endif |
| |
| Isqrt_n(&s, &exp); |
| s = L_shl(s, (exp + 5)); |
| k_cn = extract_h(L_add(s, 0x8000)); |
| |
| /* set k_dn = 32..512 (ener_dn = 2^30..2^22) */ |
| #ifdef ASM_OPT /* asm optimization branch */ |
| s = Dot_product12_asm(dn, dn, L_SUBFR, &exp); |
| #else |
| s = Dot_product12(dn, dn, L_SUBFR, &exp); |
| #endif |
| |
| Isqrt_n(&s, &exp); |
| k_dn = voround(L_shl(s, (exp + 5 + 3))); /* k_dn = 256..4096 */ |
| k_dn = vo_mult_r(alp, k_dn); /* alp in Q12 */ |
| |
| /* mix normalized cn[] and dn[] */ |
| p0 = cn; |
| p1 = dn; |
| p2 = dn2; |
| |
| for (i = 0; i < L_SUBFR/4; i++) |
| { |
| s = L_add((k_cn* (*p0++)), (k_dn * (*p1++))); |
| *p2++ = s >> 7; |
| s = L_add((k_cn* (*p0++)), (k_dn * (*p1++))); |
| *p2++ = s >> 7; |
| s = L_add((k_cn* (*p0++)), (k_dn * (*p1++))); |
| *p2++ = s >> 7; |
| s = L_add((k_cn* (*p0++)), (k_dn * (*p1++))); |
| *p2++ = s >> 7; |
| } |
| |
| /* set sign according to dn2[] = k_cn*cn[] + k_dn*dn[] */ |
| for(i = 0; i < L_SUBFR; i++) |
| { |
| val = dn[i]; |
| ps = dn2[i]; |
| if (ps >= 0) |
| { |
| sign[i] = 32767; /* sign = +1 (Q12) */ |
| vec[i] = -32768; |
| } else |
| { |
| sign[i] = -32768; /* sign = -1 (Q12) */ |
| vec[i] = 32767; |
| dn[i] = -val; |
| dn2[i] = -ps; |
| } |
| } |
| /*----------------------------------------------------------------* |
| * Select NB_MAX position per track according to max of dn2[]. * |
| *----------------------------------------------------------------*/ |
| pos = 0; |
| for (i = 0; i < NB_TRACK; i++) |
| { |
| for (k = 0; k < NB_MAX; k++) |
| { |
| ps = -1; |
| for (j = i; j < L_SUBFR; j += STEP) |
| { |
| if(dn2[j] > ps) |
| { |
| ps = dn2[j]; |
| pos = j; |
| } |
| } |
| dn2[pos] = (k - NB_MAX); /* dn2 < 0 when position is selected */ |
| if (k == 0) |
| { |
| pos_max[i] = pos; |
| } |
| } |
| } |
| |
| /*--------------------------------------------------------------* |
| * Scale h[] to avoid overflow and to get maximum of precision * |
| * on correlation. * |
| * * |
| * Maximum of h[] (h[0]) is fixed to 2048 (MAX16 / 16). * |
| * ==> This allow addition of 16 pulses without saturation. * |
| * * |
| * Energy worst case (on resonant impulse response), * |
| * - energy of h[] is approximately MAX/16. * |
| * - During search, the energy is divided by 8 to avoid * |
| * overflow on "alp". (energy of h[] = MAX/128). * |
| * ==> "alp" worst case detected is 22854 on sinusoidal wave. * |
| *--------------------------------------------------------------*/ |
| |
| /* impulse response buffer for fast computation */ |
| |
| h = h_buf; |
| h_inv = h_buf + (2 * L_SUBFR); |
| L_tmp = 0; |
| for (i = 0; i < L_SUBFR; i++) |
| { |
| *h++ = 0; |
| *h_inv++ = 0; |
| L_tmp = L_add(L_tmp, (H[i] * H[i]) << 1); |
| } |
| /* scale h[] down (/2) when energy of h[] is high with many pulses used */ |
| val = extract_h(L_tmp); |
| h_shift = 0; |
| |
| if ((nb_pulse >= 12) && (val > 1024)) |
| { |
| h_shift = 1; |
| } |
| p0 = H; |
| p1 = h; |
| p2 = h_inv; |
| |
| for (i = 0; i < L_SUBFR/4; i++) |
| { |
| *p1 = *p0++ >> h_shift; |
| *p2++ = -(*p1++); |
| *p1 = *p0++ >> h_shift; |
| *p2++ = -(*p1++); |
| *p1 = *p0++ >> h_shift; |
| *p2++ = -(*p1++); |
| *p1 = *p0++ >> h_shift; |
| *p2++ = -(*p1++); |
| } |
| |
| /*------------------------------------------------------------* |
| * Compute rrixix[][] needed for the codebook search. * |
| * This algorithm compute impulse response energy of all * |
| * positions (16) in each track (4). Total = 4x16 = 64. * |
| *------------------------------------------------------------*/ |
| |
| /* storage order --> i3i3, i2i2, i1i1, i0i0 */ |
| |
| /* Init pointers to last position of rrixix[] */ |
| p0 = &rrixix[0][NB_POS - 1]; |
| p1 = &rrixix[1][NB_POS - 1]; |
| p2 = &rrixix[2][NB_POS - 1]; |
| p3 = &rrixix[3][NB_POS - 1]; |
| |
| ptr_h1 = h; |
| cor = 0x00008000L; /* for rounding */ |
| for (i = 0; i < NB_POS; i++) |
| { |
| cor = L_add(cor, vo_L_mult((*ptr_h1), (*ptr_h1))); |
| ptr_h1++; |
| *p3-- = extract_h(cor); |
| cor = L_add(cor, vo_L_mult((*ptr_h1), (*ptr_h1))); |
| ptr_h1++; |
| *p2-- = extract_h(cor); |
| cor = L_add(cor, vo_L_mult((*ptr_h1), (*ptr_h1))); |
| ptr_h1++; |
| *p1-- = extract_h(cor); |
| cor = L_add(cor, vo_L_mult((*ptr_h1), (*ptr_h1))); |
| ptr_h1++; |
| *p0-- = extract_h(cor); |
| } |
| |
| /*------------------------------------------------------------* |
| * Compute rrixiy[][] needed for the codebook search. * |
| * This algorithm compute correlation between 2 pulses * |
| * (2 impulses responses) in 4 possible adjacents tracks. * |
| * (track 0-1, 1-2, 2-3 and 3-0). Total = 4x16x16 = 1024. * |
| *------------------------------------------------------------*/ |
| |
| /* storage order --> i2i3, i1i2, i0i1, i3i0 */ |
| |
| pos = MSIZE - 1; |
| ptr_hf = h + 1; |
| |
| for (k = 0; k < NB_POS; k++) |
| { |
| p3 = &rrixiy[2][pos]; |
| p2 = &rrixiy[1][pos]; |
| p1 = &rrixiy[0][pos]; |
| p0 = &rrixiy[3][pos - NB_POS]; |
| |
| cor = 0x00008000L; /* for rounding */ |
| ptr_h1 = h; |
| ptr_h2 = ptr_hf; |
| |
| for (i = k + 1; i < NB_POS; i++) |
| { |
| cor = L_add(cor, vo_L_mult((*ptr_h1), (*ptr_h2))); |
| ptr_h1++; |
| ptr_h2++; |
| *p3 = extract_h(cor); |
| cor = L_add(cor, vo_L_mult((*ptr_h1), (*ptr_h2))); |
| ptr_h1++; |
| ptr_h2++; |
| *p2 = extract_h(cor); |
| cor = L_add(cor, vo_L_mult((*ptr_h1), (*ptr_h2))); |
| ptr_h1++; |
| ptr_h2++; |
| *p1 = extract_h(cor); |
| cor = L_add(cor, vo_L_mult((*ptr_h1), (*ptr_h2))); |
| ptr_h1++; |
| ptr_h2++; |
| *p0 = extract_h(cor); |
| |
| p3 -= (NB_POS + 1); |
| p2 -= (NB_POS + 1); |
| p1 -= (NB_POS + 1); |
| p0 -= (NB_POS + 1); |
| } |
| cor = L_add(cor, vo_L_mult((*ptr_h1), (*ptr_h2))); |
| ptr_h1++; |
| ptr_h2++; |
| *p3 = extract_h(cor); |
| cor = L_add(cor, vo_L_mult((*ptr_h1), (*ptr_h2))); |
| ptr_h1++; |
| ptr_h2++; |
| *p2 = extract_h(cor); |
| cor = L_add(cor, vo_L_mult((*ptr_h1), (*ptr_h2))); |
| ptr_h1++; |
| ptr_h2++; |
| *p1 = extract_h(cor); |
| |
| pos -= NB_POS; |
| ptr_hf += STEP; |
| } |
| |
| /* storage order --> i3i0, i2i3, i1i2, i0i1 */ |
| |
| pos = MSIZE - 1; |
| ptr_hf = h + 3; |
| |
| for (k = 0; k < NB_POS; k++) |
| { |
| p3 = &rrixiy[3][pos]; |
| p2 = &rrixiy[2][pos - 1]; |
| p1 = &rrixiy[1][pos - 1]; |
| p0 = &rrixiy[0][pos - 1]; |
| |
| cor = 0x00008000L; /* for rounding */ |
| ptr_h1 = h; |
| ptr_h2 = ptr_hf; |
| |
| for (i = k + 1; i < NB_POS; i++) |
| { |
| cor = L_add(cor, vo_L_mult((*ptr_h1), (*ptr_h2))); |
| ptr_h1++; |
| ptr_h2++; |
| *p3 = extract_h(cor); |
| cor = L_add(cor, vo_L_mult((*ptr_h1), (*ptr_h2))); |
| ptr_h1++; |
| ptr_h2++; |
| *p2 = extract_h(cor); |
| cor = L_add(cor, vo_L_mult((*ptr_h1), (*ptr_h2))); |
| ptr_h1++; |
| ptr_h2++; |
| *p1 = extract_h(cor); |
| cor = L_add(cor, vo_L_mult((*ptr_h1), (*ptr_h2))); |
| ptr_h1++; |
| ptr_h2++; |
| *p0 = extract_h(cor); |
| |
| p3 -= (NB_POS + 1); |
| p2 -= (NB_POS + 1); |
| p1 -= (NB_POS + 1); |
| p0 -= (NB_POS + 1); |
| } |
| cor = L_add(cor, vo_L_mult((*ptr_h1), (*ptr_h2))); |
| ptr_h1++; |
| ptr_h2++; |
| *p3 = extract_h(cor); |
| |
| pos--; |
| ptr_hf += STEP; |
| } |
| |
| /*------------------------------------------------------------* |
| * Modification of rrixiy[][] to take signs into account. * |
| *------------------------------------------------------------*/ |
| |
| p0 = &rrixiy[0][0]; |
| |
| for (k = 0; k < NB_TRACK; k++) |
| { |
| j_temp = (k + 1)&0x03; |
| for (i = k; i < L_SUBFR; i += STEP) |
| { |
| psign = sign; |
| if (psign[i] < 0) |
| { |
| psign = vec; |
| } |
| j = j_temp; |
| for (; j < L_SUBFR; j += STEP) |
| { |
| *p0 = vo_mult(*p0, psign[j]); |
| p0++; |
| } |
| } |
| } |
| |
| /*-------------------------------------------------------------------* |
| * Deep first search * |
| *-------------------------------------------------------------------*/ |
| |
| psk = -1; |
| alpk = 1; |
| |
| for (k = 0; k < nbiter; k++) |
| { |
| j_temp = k<<2; |
| for (i = 0; i < nb_pulse; i++) |
| ipos[i] = tipos[j_temp + i]; |
| |
| if(nbbits == 20) |
| { |
| pos = 0; |
| ps = 0; |
| alp = 0; |
| for (i = 0; i < L_SUBFR; i++) |
| { |
| vec[i] = 0; |
| } |
| } else if ((nbbits == 36) || (nbbits == 44)) |
| { |
| /* first stage: fix 2 pulses */ |
| pos = 2; |
| |
| ix = ind[0] = pos_max[ipos[0]]; |
| iy = ind[1] = pos_max[ipos[1]]; |
| ps = dn[ix] + dn[iy]; |
| i = ix >> 2; /* ix / STEP */ |
| j = iy >> 2; /* iy / STEP */ |
| s = rrixix[ipos[0]][i] << 13; |
| s += rrixix[ipos[1]][j] << 13; |
| i = (i << 4) + j; /* (ix/STEP)*NB_POS + (iy/STEP) */ |
| s += rrixiy[ipos[0]][i] << 14; |
| alp = (s + 0x8000) >> 16; |
| if (sign[ix] < 0) |
| p0 = h_inv - ix; |
| else |
| p0 = h - ix; |
| if (sign[iy] < 0) |
| p1 = h_inv - iy; |
| else |
| p1 = h - iy; |
| |
| for (i = 0; i < L_SUBFR; i++) |
| { |
| vec[i] = (*p0++) + (*p1++); |
| } |
| |
| if(nbbits == 44) |
| { |
| ipos[8] = 0; |
| ipos[9] = 1; |
| } |
| } else |
| { |
| /* first stage: fix 4 pulses */ |
| pos = 4; |
| |
| ix = ind[0] = pos_max[ipos[0]]; |
| iy = ind[1] = pos_max[ipos[1]]; |
| i = ind[2] = pos_max[ipos[2]]; |
| j = ind[3] = pos_max[ipos[3]]; |
| ps = add1(add1(add1(dn[ix], dn[iy]), dn[i]), dn[j]); |
| |
| if (sign[ix] < 0) |
| p0 = h_inv - ix; |
| else |
| p0 = h - ix; |
| |
| if (sign[iy] < 0) |
| p1 = h_inv - iy; |
| else |
| p1 = h - iy; |
| |
| if (sign[i] < 0) |
| p2 = h_inv - i; |
| else |
| p2 = h - i; |
| |
| if (sign[j] < 0) |
| p3 = h_inv - j; |
| else |
| p3 = h - j; |
| |
| L_tmp = 0L; |
| for(i = 0; i < L_SUBFR; i++) |
| { |
| Word32 vecSq2; |
| vec[i] = add1(add1(add1(*p0++, *p1++), *p2++), *p3++); |
| vecSq2 = (vec[i] * vec[i]) << 1; |
| if (vecSq2 > 0 && L_tmp > INT_MAX - vecSq2) { |
| L_tmp = INT_MAX; |
| } else if (vecSq2 < 0 && L_tmp < INT_MIN - vecSq2) { |
| L_tmp = INT_MIN; |
| } else { |
| L_tmp += vecSq2; |
| } |
| } |
| |
| alp = ((L_tmp >> 3) + 0x8000) >> 16; |
| |
| if(nbbits == 72) |
| { |
| ipos[16] = 0; |
| ipos[17] = 1; |
| } |
| } |
| |
| /* other stages of 2 pulses */ |
| |
| for (j = pos, st = 0; j < nb_pulse; j += 2, st++) |
| { |
| /*--------------------------------------------------* |
| * Calculate correlation of all possible positions * |
| * of the next 2 pulses with previous fixed pulses. * |
| * Each pulse can have 16 possible positions. * |
| *--------------------------------------------------*/ |
| if(ipos[j] == 3) |
| { |
| cor_h_vec_30(h, vec, ipos[j], sign, rrixix, cor_x, cor_y); |
| } |
| else |
| { |
| #ifdef ASM_OPT /* asm optimization branch */ |
| cor_h_vec_012_asm(h, vec, ipos[j], sign, rrixix, cor_x, cor_y); |
| #else |
| cor_h_vec_012(h, vec, ipos[j], sign, rrixix, cor_x, cor_y); |
| #endif |
| } |
| /*--------------------------------------------------* |
| * Find best positions of 2 pulses. * |
| *--------------------------------------------------*/ |
| search_ixiy(nbpos[st], ipos[j], ipos[j + 1], &ps, &alp, |
| &ix, &iy, dn, dn2, cor_x, cor_y, rrixiy); |
| |
| ind[j] = ix; |
| ind[j + 1] = iy; |
| |
| if (sign[ix] < 0) |
| p0 = h_inv - ix; |
| else |
| p0 = h - ix; |
| if (sign[iy] < 0) |
| p1 = h_inv - iy; |
| else |
| p1 = h - iy; |
| |
| for (i = 0; i < L_SUBFR; i+=4) |
| { |
| vec[i] += add1((*p0++), (*p1++)); |
| vec[i+1] += add1((*p0++), (*p1++)); |
| vec[i+2] += add1((*p0++), (*p1++)); |
| vec[i+3] += add1((*p0++), (*p1++)); |
| } |
| } |
| /* memorise the best codevector */ |
| ps = vo_mult(ps, ps); |
| s = L_sub(vo_L_mult(alpk, ps), vo_L_mult(psk, alp)); |
| if (s > 0) |
| { |
| psk = ps; |
| alpk = alp; |
| for (i = 0; i < nb_pulse; i++) |
| { |
| codvec[i] = ind[i]; |
| } |
| for (i = 0; i < L_SUBFR; i++) |
| { |
| y[i] = vec[i]; |
| } |
| } |
| } |
| /*-------------------------------------------------------------------* |
| * Build the codeword, the filtered codeword and index of codevector.* |
| *-------------------------------------------------------------------*/ |
| for (i = 0; i < NPMAXPT * NB_TRACK; i++) |
| { |
| ind[i] = -1; |
| } |
| for (i = 0; i < L_SUBFR; i++) |
| { |
| code[i] = 0; |
| y[i] = vo_shr_r(y[i], 3); /* Q12 to Q9 */ |
| } |
| val = (512 >> h_shift); /* codeword in Q9 format */ |
| for (k = 0; k < nb_pulse; k++) |
| { |
| i = codvec[k]; /* read pulse position */ |
| j = sign[i]; /* read sign */ |
| index = i >> 2; /* index = pos of pulse (0..15) */ |
| track = (Word16) (i & 0x03); /* track = i % NB_TRACK (0..3) */ |
| |
| if (j > 0) |
| { |
| code[i] += val; |
| codvec[k] += 128; |
| } else |
| { |
| code[i] -= val; |
| index += NB_POS; |
| } |
| |
| i = (Word16)((vo_L_mult(track, NPMAXPT) >> 1)); |
| |
| while (i < NPMAXPT * NB_TRACK && ind[i] >= 0) |
| { |
| i += 1; |
| } |
| if (i < NPMAXPT * NB_TRACK) { |
| ind[i] = index; |
| } else { |
| ALOGE("b/132647222, OOB access in ind array track=%d i=%d", track, i); |
| android_errorWriteLog(0x534e4554, "132647222"); |
| } |
| } |
| |
| k = 0; |
| /* Build index of codevector */ |
| if(nbbits == 20) |
| { |
| for (track = 0; track < NB_TRACK; track++) |
| { |
| _index[track] = (Word16)(quant_1p_N1(ind[k], 4)); |
| k += NPMAXPT; |
| } |
| } else if(nbbits == 36) |
| { |
| for (track = 0; track < NB_TRACK; track++) |
| { |
| _index[track] = (Word16)(quant_2p_2N1(ind[k], ind[k + 1], 4)); |
| k += NPMAXPT; |
| } |
| } else if(nbbits == 44) |
| { |
| for (track = 0; track < NB_TRACK - 2; track++) |
| { |
| _index[track] = (Word16)(quant_3p_3N1(ind[k], ind[k + 1], ind[k + 2], 4)); |
| k += NPMAXPT; |
| } |
| for (track = 2; track < NB_TRACK; track++) |
| { |
| _index[track] = (Word16)(quant_2p_2N1(ind[k], ind[k + 1], 4)); |
| k += NPMAXPT; |
| } |
| } else if(nbbits == 52) |
| { |
| for (track = 0; track < NB_TRACK; track++) |
| { |
| _index[track] = (Word16)(quant_3p_3N1(ind[k], ind[k + 1], ind[k + 2], 4)); |
| k += NPMAXPT; |
| } |
| } else if(nbbits == 64) |
| { |
| for (track = 0; track < NB_TRACK; track++) |
| { |
| L_index = quant_4p_4N(&ind[k], 4); |
| _index[track] = (Word16)((L_index >> 14) & 3); |
| _index[track + NB_TRACK] = (Word16)(L_index & 0x3FFF); |
| k += NPMAXPT; |
| } |
| } else if(nbbits == 72) |
| { |
| for (track = 0; track < NB_TRACK - 2; track++) |
| { |
| L_index = quant_5p_5N(&ind[k], 4); |
| _index[track] = (Word16)((L_index >> 10) & 0x03FF); |
| _index[track + NB_TRACK] = (Word16)(L_index & 0x03FF); |
| k += NPMAXPT; |
| } |
| for (track = 2; track < NB_TRACK; track++) |
| { |
| L_index = quant_4p_4N(&ind[k], 4); |
| _index[track] = (Word16)((L_index >> 14) & 3); |
| _index[track + NB_TRACK] = (Word16)(L_index & 0x3FFF); |
| k += NPMAXPT; |
| } |
| } else if(nbbits == 88) |
| { |
| for (track = 0; track < NB_TRACK; track++) |
| { |
| L_index = quant_6p_6N_2(&ind[k], 4); |
| _index[track] = (Word16)((L_index >> 11) & 0x07FF); |
| _index[track + NB_TRACK] = (Word16)(L_index & 0x07FF); |
| k += NPMAXPT; |
| } |
| } |
| return; |
| } |
| |
| |
| /*-------------------------------------------------------------------* |
| * Function cor_h_vec() * |
| * ~~~~~~~~~~~~~~~~~~~~~ * |
| * Compute correlations of h[] with vec[] for the specified track. * |
| *-------------------------------------------------------------------*/ |
| void cor_h_vec_30( |
| Word16 h[], /* (i) scaled impulse response */ |
| Word16 vec[], /* (i) scaled vector (/8) to correlate with h[] */ |
| Word16 track, /* (i) track to use */ |
| Word16 sign[], /* (i) sign vector */ |
| Word16 rrixix[][NB_POS], /* (i) correlation of h[x] with h[x] */ |
| Word16 cor_1[], /* (o) result of correlation (NB_POS elements) */ |
| Word16 cor_2[] /* (o) result of correlation (NB_POS elements) */ |
| ) |
| { |
| Word32 i, j, pos, corr; |
| Word16 *p0, *p1, *p2,*p3,*cor_x,*cor_y; |
| Word32 L_sum1,L_sum2; |
| cor_x = cor_1; |
| cor_y = cor_2; |
| p0 = rrixix[track]; |
| p3 = rrixix[0]; |
| pos = track; |
| |
| for (i = 0; i < NB_POS; i+=2) |
| { |
| L_sum1 = L_sum2 = 0L; |
| p1 = h; |
| p2 = &vec[pos]; |
| for (j=pos;j < L_SUBFR; j++) |
| { |
| L_sum1 = L_add(L_sum1, *p1 * *p2); |
| p2-=3; |
| L_sum2 = L_add(L_sum2, *p1++ * *p2); |
| p2+=4; |
| } |
| p2-=3; |
| L_sum2 = L_add(L_sum2, *p1++ * *p2++); |
| L_sum2 = L_add(L_sum2, *p1++ * *p2++); |
| L_sum2 = L_add(L_sum2, *p1++ * *p2++); |
| |
| L_sum1 = L_shl(L_sum1, 2); |
| L_sum2 = L_shl(L_sum2, 2); |
| |
| corr = voround(L_sum1); |
| *cor_x++ = mult(corr, sign[pos]) + (*p0++); |
| corr = voround(L_sum2); |
| *cor_y++ = mult(corr, sign[pos-3]) + (*p3++); |
| pos += STEP; |
| |
| L_sum1 = L_sum2 = 0L; |
| p1 = h; |
| p2 = &vec[pos]; |
| for (j=pos;j < L_SUBFR; j++) |
| { |
| L_sum1 = L_add(L_sum1, *p1 * *p2); |
| p2-=3; |
| L_sum2 = L_add(L_sum2, *p1++ * *p2); |
| p2+=4; |
| } |
| p2-=3; |
| L_sum2 = L_add(L_sum2, *p1++ * *p2++); |
| L_sum2 = L_add(L_sum2, *p1++ * *p2++); |
| L_sum2 = L_add(L_sum2, *p1++ * *p2++); |
| |
| L_sum1 = L_shl(L_sum1, 2); |
| L_sum2 = L_shl(L_sum2, 2); |
| |
| corr = voround(L_sum1); |
| *cor_x++ = mult(corr, sign[pos]) + (*p0++); |
| corr = voround(L_sum2); |
| *cor_y++ = mult(corr, sign[pos-3]) + (*p3++); |
| pos += STEP; |
| } |
| return; |
| } |
| |
| void cor_h_vec_012( |
| Word16 h[], /* (i) scaled impulse response */ |
| Word16 vec[], /* (i) scaled vector (/8) to correlate with h[] */ |
| Word16 track, /* (i) track to use */ |
| Word16 sign[], /* (i) sign vector */ |
| Word16 rrixix[][NB_POS], /* (i) correlation of h[x] with h[x] */ |
| Word16 cor_1[], /* (o) result of correlation (NB_POS elements) */ |
| Word16 cor_2[] /* (o) result of correlation (NB_POS elements) */ |
| ) |
| { |
| Word32 i, j, pos, corr; |
| Word16 *p0, *p1, *p2,*p3,*cor_x,*cor_y; |
| Word32 L_sum1,L_sum2; |
| cor_x = cor_1; |
| cor_y = cor_2; |
| p0 = rrixix[track]; |
| p3 = rrixix[track+1]; |
| pos = track; |
| |
| for (i = 0; i < NB_POS; i+=2) |
| { |
| L_sum1 = L_sum2 = 0L; |
| p1 = h; |
| p2 = &vec[pos]; |
| for (j=62-pos ;j >= 0; j--) |
| { |
| L_sum1 = L_add(L_sum1, *p1 * *p2++); |
| L_sum2 = L_add(L_sum2, *p1++ * *p2); |
| } |
| L_sum1 = L_add(L_sum1, *p1 * *p2); |
| L_sum1 = L_shl(L_sum1, 2); |
| L_sum2 = L_shl(L_sum2, 2); |
| |
| corr = voround(L_sum1); |
| cor_x[i] = vo_mult(corr, sign[pos]) + (*p0++); |
| corr = voround(L_sum2); |
| cor_y[i] = vo_mult(corr, sign[pos + 1]) + (*p3++); |
| pos += STEP; |
| |
| L_sum1 = L_sum2 = 0L; |
| p1 = h; |
| p2 = &vec[pos]; |
| for (j= 62-pos;j >= 0; j--) |
| { |
| L_sum1 = L_add(L_sum1, *p1 * *p2++); |
| L_sum2 = L_add(L_sum2, *p1++ * *p2); |
| } |
| L_sum1 = L_add(L_sum1, *p1 * *p2); |
| L_sum1 = L_shl(L_sum1, 2); |
| L_sum2 = L_shl(L_sum2, 2); |
| |
| corr = voround(L_sum1); |
| cor_x[i+1] = vo_mult(corr, sign[pos]) + (*p0++); |
| corr = voround(L_sum2); |
| cor_y[i+1] = vo_mult(corr, sign[pos + 1]) + (*p3++); |
| pos += STEP; |
| } |
| return; |
| } |
| |
| /*-------------------------------------------------------------------* |
| * Function search_ixiy() * |
| * ~~~~~~~~~~~~~~~~~~~~~~~ * |
| * Find the best positions of 2 pulses in a subframe. * |
| *-------------------------------------------------------------------*/ |
| |
| void search_ixiy( |
| Word16 nb_pos_ix, /* (i) nb of pos for pulse 1 (1..8) */ |
| Word16 track_x, /* (i) track of pulse 1 */ |
| Word16 track_y, /* (i) track of pulse 2 */ |
| Word16 * ps, /* (i/o) correlation of all fixed pulses */ |
| Word16 * alp, /* (i/o) energy of all fixed pulses */ |
| Word16 * ix, /* (o) position of pulse 1 */ |
| Word16 * iy, /* (o) position of pulse 2 */ |
| Word16 dn[], /* (i) corr. between target and h[] */ |
| Word16 dn2[], /* (i) vector of selected positions */ |
| Word16 cor_x[], /* (i) corr. of pulse 1 with fixed pulses */ |
| Word16 cor_y[], /* (i) corr. of pulse 2 with fixed pulses */ |
| Word16 rrixiy[][MSIZE] /* (i) corr. of pulse 1 with pulse 2 */ |
| ) |
| { |
| Word32 x, y, pos, thres_ix; |
| Word16 ps1, ps2, sq, sqk; |
| Word16 alp_16, alpk; |
| Word16 *p0, *p1, *p2; |
| Word32 s, alp0, alp1, alp2; |
| |
| p0 = cor_x; |
| p1 = cor_y; |
| p2 = rrixiy[track_x]; |
| |
| thres_ix = nb_pos_ix - NB_MAX; |
| |
| alp0 = L_deposit_h(*alp); |
| alp0 = (alp0 + 0x00008000L); /* for rounding */ |
| |
| sqk = -1; |
| alpk = 1; |
| |
| for (x = track_x; x < L_SUBFR; x += STEP) |
| { |
| ps1 = *ps + dn[x]; |
| alp1 = L_add(alp0, ((*p0++)<<13)); |
| |
| if (dn2[x] < thres_ix) |
| { |
| pos = -1; |
| for (y = track_y; y < L_SUBFR; y += STEP) |
| { |
| ps2 = add1(ps1, dn[y]); |
| |
| alp2 = L_add(alp1, ((*p1++)<<13)); |
| alp2 = L_add(alp2, ((*p2++)<<14)); |
| alp_16 = extract_h(alp2); |
| sq = vo_mult(ps2, ps2); |
| s = L_sub(vo_L_mult(alpk, sq), L_mult(sqk, alp_16)); |
| |
| if (s > 0) |
| { |
| sqk = sq; |
| alpk = alp_16; |
| pos = y; |
| } |
| } |
| p1 -= NB_POS; |
| |
| if (pos >= 0) |
| { |
| *ix = x; |
| *iy = pos; |
| } |
| } else |
| { |
| p2 += NB_POS; |
| } |
| } |
| |
| *ps = add1(*ps, add1(dn[*ix], dn[*iy])); |
| *alp = alpk; |
| |
| return; |
| } |
| |
| |
| |
| |
