c4t64fx.c

通讯协议

            /*--------------------------------------------------*
            * Calculate correlation of all possible positions  *
            * of the next 2 pulses with previous fixed pulses. *
            * Each pulse can have 16 possible positions.       *
            *--------------------------------------------------*/

            cor_h_vec(h, vec, ipos[j], sign, rrixix, cor_x);
            cor_h_vec(h, vec, ipos[j + 1], sign, rrixix, cor_y);

            /*--------------------------------------------------*
            * 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;                   move16();
            ind[j + 1] = iy;               move16();

            test();move16();move16();
            if (sign[ix] < 0)
                p0 = h_inv - ix;
            else
                p0 = h - ix;
            test();move16();move16();
            if (sign[iy] < 0)
                p1 = h_inv - iy;
            else
                p1 = h - iy;

            for (i = 0; i < L_SUBFR; i++)
            {
                vec[i] = add(vec[i], add(*p0++, *p1++));        /* can saturate here. */
                move16();
            }
        }

        /* memorise the best codevector */

        ps = mult(ps, ps);
        s = L_msu(L_mult(alpk, ps), psk, alp);
        test();
        if (s > 0)
        {
            psk = ps;                      move16();
            alpk = alp;                    move16();
            for (i = 0; i < nb_pulse; i++)
            {
                codvec[i] = ind[i];        move16();
            }
            for (i = 0; i < L_SUBFR; i++)
            {
                y[i] = vec[i];             move16();
            }
        }
    }

    /*-------------------------------------------------------------------*
     * Build the codeword, the filtered codeword and index of codevector.*
     *-------------------------------------------------------------------*/

    for (i = 0; i < NPMAXPT * NB_TRACK; i++)
    {
        ind[i] = -1;                       move16();
    }
    for (i = 0; i < L_SUBFR; i++)
    {
        code[i] = 0;                       move16();
        y[i] = shr_r(y[i], 3);             move16();  /* Q12 to Q9 */
    }

    val = shr(512, h_shift);               /* codeword in Q9 format */

    for (k = 0; k < nb_pulse; k++)
    {
        i = codvec[k];                     move16();  /* read pulse position */
        j = sign[i];                       move16();  /* read sign           */

        index = shr(i, 2);                 /* index = pos of pulse (0..15) */
        track = (Word16) (i & 0x03);       logic16();  /* track = i % NB_TRACK (0..3)  */

        if (j > 0)
        {
            code[i] = add(code[i], val);   move16();
            codvec[k] = add(codvec[k], (2 * L_SUBFR));  move16();
        } else
        {
            code[i] = sub(code[i], val);   move16();
            index = add(index, NB_POS);    move16();
        }

        i = extract_l(L_shr(L_mult(track, NPMAXPT), 1));

        test();move16();
        while (ind[i] >= 0)
        {
            i = add(i, 1);
        }
        ind[i] = index;                    move16();
    }

    k = 0;                                 move16();
    /* Build index of codevector */
    test();test();test();test();test();test();test();
    if (sub(nbbits, 20) == 0)
    {
        for (track = 0; track < NB_TRACK; track++)
        {
            _index[track] = extract_l(quant_1p_N1(ind[k], 4));
            k += NPMAXPT;
        }
    } else if (sub(nbbits, 36) == 0)
    {
        for (track = 0; track < NB_TRACK; track++)
        {
            _index[track] = extract_l(quant_2p_2N1(ind[k], ind[k + 1], 4));
            k += NPMAXPT;
        }
    } else if (sub(nbbits, 44) == 0)
    {
        for (track = 0; track < NB_TRACK - 2; track++)
        {
            _index[track] = extract_l(quant_3p_3N1(ind[k], ind[k + 1], ind[k + 2], 4));
            k += NPMAXPT;
        }
        for (track = 2; track < NB_TRACK; track++)
        {
            _index[track] = extract_l(quant_2p_2N1(ind[k], ind[k + 1], 4));
            k += NPMAXPT;
        }
    } else if (sub(nbbits, 52) == 0)
    {
        for (track = 0; track < NB_TRACK; track++)
        {
            _index[track] = extract_l(quant_3p_3N1(ind[k], ind[k + 1], ind[k + 2], 4));
            k += NPMAXPT;
        }
    } else if (sub(nbbits, 64) == 0)
    {
        for (track = 0; track < NB_TRACK; track++)
        {
            L_index = quant_4p_4N(&ind[k], 4);
            _index[track] = extract_l(L_shr(L_index, 14) & 3);
            _index[track + NB_TRACK] = extract_l(L_index & 0x3FFF);
            k += NPMAXPT;
        }
    } else if (sub(nbbits, 72) == 0)
    {
        for (track = 0; track < NB_TRACK - 2; track++)
        {
            L_index = quant_5p_5N(&ind[k], 4);
            _index[track] = extract_l(L_shr(L_index, 10) & 0x03FF);
            _index[track + NB_TRACK] = extract_l(L_index & 0x03FF);
            k += NPMAXPT;
        }
        for (track = 2; track < NB_TRACK; track++)
        {
            L_index = quant_4p_4N(&ind[k], 4);
            _index[track] = extract_l(L_shr(L_index, 14) & 3);
            _index[track + NB_TRACK] = extract_l(L_index & 0x3FFF);
            k += NPMAXPT;
        }
    } else if (sub(nbbits, 88) == 0)
    {
        for (track = 0; track < NB_TRACK; track++)
        {
            L_index = quant_6p_6N_2(&ind[k], 4);
            _index[track] = extract_l(L_shr(L_index, 11) & 0x07FF);
            _index[track + NB_TRACK] = extract_l(L_index & 0x07FF);
            k += NPMAXPT;
        }
    }
    return;
}


/*-------------------------------------------------------------------*
 * Function  cor_h_vec()                                             *
 * ~~~~~~~~~~~~~~~~~~~~~                                             *
 * Compute correlations of h[] with vec[] for the specified track.   *
 *-------------------------------------------------------------------*/
static void cor_h_vec(
     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[]                          /* (o) result of correlation (NB_POS elements) */
)
{
    Word16 i, j, pos, corr;
    Word16 *p0, *p1, *p2;
    Word32 L_sum;

    p0 = rrixix[track];                    move16();

    pos = track;                           move16();
    for (i = 0; i < NB_POS; i++, pos += STEP)
    {
        L_sum = 0L;                        move32();
        p1 = h;                            move16();
        p2 = &vec[pos];                    move16();
        for (j = pos; j < L_SUBFR; j++)
            L_sum = L_mac(L_sum, *p1++, *p2++);

        L_sum = L_shl(L_sum, 1);

        corr = round(L_sum);

        cor[i] = add(mult(corr, sign[pos]), *p0++);     move16();

    }

    return;
}


/*-------------------------------------------------------------------*
 * Function  search_ixiy()                                           *
 * ~~~~~~~~~~~~~~~~~~~~~~~                                           *
 * Find the best positions of 2 pulses in a subframe.                *
 *-------------------------------------------------------------------*/

static 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   */
)
{
    Word16 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;                            move16();
    p1 = cor_y;                            move16();
    p2 = rrixiy[track_x];                  move16();

    thres_ix = sub(nb_pos_ix, NB_MAX);

    alp0 = L_deposit_h(*alp);
    alp0 = L_add(alp0, 0x00008000L);       /* for rounding */

    sqk = -1;                              move16();
    alpk = 1;                              move16();

    for (x = track_x; x < L_SUBFR; x += STEP)
    {
        ps1 = add(*ps, dn[x]);
        alp1 = L_mac(alp0, *p0++, 4096);

        test();
        if (sub(dn2[x], thres_ix) < 0)
        {
            pos = -1;                      move16();
            for (y = track_y; y < L_SUBFR; y += STEP)
            {
                ps2 = add(ps1, dn[y]);
                alp2 = L_mac(alp1, *p1++, 4096);
                alp2 = L_mac(alp2, *p2++, 8192);
                alp_16 = extract_h(alp2);

                sq = mult(ps2, ps2);

                s = L_msu(L_mult(alpk, sq), sqk, alp_16);

                test();
                if (s > 0)
                {
                    sqk = sq;              move16();
                    alpk = alp_16;         move16();
                    pos = y;               move16();
                }
            }
            p1 -= NB_POS;

            test();
            if (pos >= 0)
            {
                *ix = x;                   move16();
                *iy = pos;                 move16();
            }
        } else
        {
            p2 += NB_POS;
        }
    }

    *ps = add(*ps, add(dn[*ix], dn[*iy])); move16();
    *alp = alpk;                           move16();

    return;
}