q_pulse.c

通讯协议

/*--------------------------------------------------------------------------*
 *                         Q_PULSE.C										*
 *--------------------------------------------------------------------------*
 * Coding and decodeing of algebraic codebook                               *
 *--------------------------------------------------------------------------*/

#include <stdio.h>

#include "typedef.h"
#include "basic_op.h"
#include "count.h"

#include "q_pulse.h"


#define NB_POS 16                          /* pos in track, mask for sign bit */


Word32 quant_1p_N1(                        /* (o) return N+1 bits             */
     Word16 pos,                           /* (i) position of the pulse       */
     Word16 N)                             /* (i) number of bits for position */
{
    Word16 mask;
    Word32 index;

    mask = sub(shl(1, N), 1);              /* mask = ((1<<N)-1); */
    /*-------------------------------------------------------*
     * Quantization of 1 pulse with N+1 bits:                *
     *-------------------------------------------------------*/
    index = L_deposit_l((Word16) (pos & mask));
    test();
    if ((pos & NB_POS) != 0)
    {
        index = L_add(index, L_deposit_l(shl(1, N)));   /* index += 1 << N; */
    }
    return (index);
}

void dec_1p_N1(Word32 index, Word16 N, Word16 offset, Word16 pos[])
{
    Word16 pos1;
    Word32 mask, i;

    mask = L_deposit_l(sub(shl(1, N), 1));   /* mask = ((1<<N)-1); */
    /*-------------------------------------------------------*
     * Decode 1 pulse with N+1 bits:                         *
     *-------------------------------------------------------*/
    pos1 = add(extract_l(index & mask), offset);        /* pos1 = ((index & mask) + offset); */
    i = (L_shr(index, N) & 1L);              /* i = ((index >> N) & 1); */
    test();
    if (L_sub(i, 1) == 0)
    {
        pos1 = add(pos1, NB_POS);
    }
    pos[0] = pos1;                         move16();

    return;
}


Word32 quant_2p_2N1(                       /* (o) return (2*N)+1 bits         */
     Word16 pos1,                          /* (i) position of the pulse 1     */
     Word16 pos2,                          /* (i) position of the pulse 2     */
     Word16 N)                             /* (i) number of bits for position */
{
    Word16 mask, tmp;
    Word32 index;

    mask = sub(shl(1, N), 1);              /* mask = ((1<<N)-1); */
    /*-------------------------------------------------------*
     * Quantization of 2 pulses with 2*N+1 bits:             *
     *-------------------------------------------------------*/
    test();logic16();logic16();
    if (((pos2 ^ pos1) & NB_POS) == 0)
    {
        /* sign of 1st pulse == sign of 2th pulse */
        test();
        if (sub(pos1, pos2) <= 0)          /* ((pos1 - pos2) <= 0) */
        {
            /* index = ((pos1 & mask) << N) + (pos2 & mask); */
            index = L_deposit_l(add(shl(((Word16) (pos1 & mask)), N), ((Word16) (pos2 & mask))));
        } else
        {
            /* ((pos2 & mask) << N) + (pos1 & mask); */
            index = L_deposit_l(add(shl(((Word16) (pos2 & mask)), N), ((Word16) (pos1 & mask))));
        }
        test();logic16();
        if ((pos1 & NB_POS) != 0)
        {
            tmp = shl(N, 1);
            index = L_add(index, L_shl(1L, tmp));       /* index += 1 << (2*N); */
        }
    } else
    {
        /* sign of 1st pulse != sign of 2th pulse */
        test();logic16();logic16();
        if (sub((Word16) (pos1 & mask), (Word16) (pos2 & mask)) <= 0)
        {
            /* index = ((pos2 & mask) << N) + (pos1 & mask); */
            index = L_deposit_l(add(shl(((Word16) (pos2 & mask)), N), ((Word16) (pos1 & mask))));       logic16();logic16();
            test();logic16();
            if ((pos2 & NB_POS) != 0)
            {
                tmp = shl(N, 1);           /* index += 1 << (2*N); */
                index = L_add(index, L_shl(1L, tmp));
            }
        } else
        {
            /* index = ((pos1 & mask) << N) + (pos2 & mask);	 */
            index = L_deposit_l(add(shl(((Word16) (pos1 & mask)), N), ((Word16) (pos2 & mask))));       logic16();logic16();
            test();logic16();
            if ((pos1 & NB_POS) != 0)
            {
                tmp = shl(N, 1);
                index = L_add(index, L_shl(1, tmp));    /* index += 1 << (2*N); */
            }
        }
    }
    return (index);
}

void dec_2p_2N1(Word32 index, Word16 N, Word16 offset, Word16 pos[])
{
    Word16 pos1, pos2, tmp;
    Word32 mask, i;

    mask = L_deposit_l(sub(shl(1, N), 1)); /* mask = ((1<<N)-1); */
    /*-------------------------------------------------------*
     * Decode 2 pulses with 2*N+1 bits:                      *
     *-------------------------------------------------------*/
    /* pos1 = (((index >> N) & mask) + offset); */
    pos1 = extract_l(L_add((L_shr(index, N) & mask), L_deposit_l(offset)));     logic16();
    tmp = shl(N, 1);
    i = (L_shr(index, tmp) & 1L);          logic16();/* i = (index >> (2*N)) & 1; */
    pos2 = add(extract_l(index & mask), offset);        logic16();/* pos2 = ((index & mask) + offset); */
    test();
    if (sub(pos2, pos1) < 0)               /* ((pos2 - pos1) < 0) */
    {
        test();
        if (L_sub(i, 1L) == 0)
        {                                  /* (i == 1) */
            pos1 = add(pos1, NB_POS);      /* pos1 += NB_POS; */
        } else
        {
            pos2 = add(pos2, NB_POS);      /* pos2 += NB_POS;	 */
        }
    } else
    {
        test();
        if (L_sub(i, 1L) == 0)
        {                                  /* (i == 1) */
            pos1 = add(pos1, NB_POS);      /* pos1 += NB_POS; */
            pos2 = add(pos2, NB_POS);      /* pos2 += NB_POS; */
        }
    }

    pos[0] = pos1;                         move16();
    pos[1] = pos2;                         move16();

    return;
}


Word32 quant_3p_3N1(                       /* (o) return (3*N)+1 bits         */
     Word16 pos1,                          /* (i) position of the pulse 1     */
     Word16 pos2,                          /* (i) position of the pulse 2     */
     Word16 pos3,                          /* (i) position of the pulse 3     */
     Word16 N)                             /* (i) number of bits for position */
{
    Word16 nb_pos;
    Word32 index;

    nb_pos = shl(1, sub(N, 1));            /* nb_pos = (1<<(N-1)); */
    /*-------------------------------------------------------*
     * Quantization of 3 pulses with 3*N+1 bits:             *
     *-------------------------------------------------------*/
    test();test();logic16();logic16();logic16();logic16();
    if (((pos1 ^ pos2) & nb_pos) == 0)
    {
        index = quant_2p_2N1(pos1, pos2, sub(N, 1));    /* index = quant_2p_2N1(pos1, pos2, (N-1)); */
        /* index += (pos1 & nb_pos) << N; */
        index = L_add(index, L_shl(L_deposit_l((Word16) (pos1 & nb_pos)), N));  logic16();
        /* index += quant_1p_N1(pos3, N) << (2*N); */
        index = L_add(index, L_shl(quant_1p_N1(pos3, N), shl(N, 1)));

    } else if (((pos1 ^ pos3) & nb_pos) == 0)
    {
        index = quant_2p_2N1(pos1, pos3, sub(N, 1));    /* index = quant_2p_2N1(pos1, pos3, (N-1)); */
        index = L_add(index, L_shl(L_deposit_l((Word16) (pos1 & nb_pos)), N));  logic16();
        /* index += (pos1 & nb_pos) << N; */
        index = L_add(index, L_shl(quant_1p_N1(pos2, N), shl(N, 1)));
        /* index += quant_1p_N1(pos2, N) <<
                                                                         * (2*N); */
    } else
    {
        index = quant_2p_2N1(pos2, pos3, sub(N, 1));    /* index = quant_2p_2N1(pos2, pos3, (N-1)); */
        /* index += (pos2 & nb_pos) << N;			 */
        index = L_add(index, L_shl(L_deposit_l((Word16) (pos2 & nb_pos)), N));  logic16();
        /* index += quant_1p_N1(pos1, N) << (2*N);	 */
        index = L_add(index, L_shl(quant_1p_N1(pos1, N), shl(N, 1)));
    }
    return (index);
}

void dec_3p_3N1(Word32 index, Word16 N, Word16 offset, Word16 pos[])
{
    Word16 j, tmp;
    Word32 mask, idx;

    /*-------------------------------------------------------*
     * Decode 3 pulses with 3*N+1 bits:                      *
     *-------------------------------------------------------*/
    tmp = sub(shl(N, 1), 1);               /* mask = ((1<<((2*N)-1))-1); */
    mask = L_sub(L_shl(1L, tmp), 1L);

    idx = index & mask;                    logic16();
    j = offset;
    tmp = sub(shl(N, 1), 1);

    test();logic16();
    if ((L_shr(index, tmp) & 1L) != 0L)
    {                                      /* if (((index >> ((2*N)-1)) & 1) == 1){ */
        j = add(j, shl(1, sub(N, 1)));     /* j += (1<<(N-1)); */
    }
    dec_2p_2N1(idx, (Word16) (N - 1), j, pos);

    mask = sub(shl(1, add(N, 1)), 1);      /* mask = ((1<<(N+1))-1); */
    tmp = shl(N, 1);                       /* idx = (index >> (2*N)) & mask; */
    idx = L_shr(index, tmp) & mask;        logic16();

    dec_1p_N1(idx, N, offset, pos + 2);    move16();

    return;
}


Word32 quant_4p_4N1(                       /* (o) return (4*N)+1 bits         */
     Word16 pos1,                          /* (i) position of the pulse 1     */
     Word16 pos2,                          /* (i) position of the pulse 2     */
     Word16 pos3,                          /* (i) position of the pulse 3     */
     Word16 pos4,                          /* (i) position of the pulse 4     */
     Word16 N)                             /* (i) number of bits for position */
{
    Word16 nb_pos;
    Word32 index;

    nb_pos = shl(1, sub(N, 1));            /* nb_pos = (1<<(N-1));  */
    /*-------------------------------------------------------*
     * Quantization of 4 pulses with 4*N+1 bits:             *
     *-------------------------------------------------------*/
    test();test();logic16();logic16();logic16();logic16();
    if (((pos1 ^ pos2) & nb_pos) == 0)
    {
        index = quant_2p_2N1(pos1, pos2, sub(N, 1));    /* index = quant_2p_2N1(pos1, pos2, (N-1)); */
        /* index += (pos1 & nb_pos) << N;	 */
        index = L_add(index, L_shl(L_deposit_l((Word16) (pos1 & nb_pos)), N));  logic16();
        /* index += quant_2p_2N1(pos3, pos4, N) << (2*N); */
        index = L_add(index, L_shl(quant_2p_2N1(pos3, pos4, N), shl(N, 1)));
    } else if (((pos1 ^ pos3) & nb_pos) == 0)
    {
        index = quant_2p_2N1(pos1, pos3, sub(N, 1));
        /* index += (pos1 & nb_pos) << N; */
        index = L_add(index, L_shl(L_deposit_l((Word16) (pos1 & nb_pos)), N));  logic16();
        /* index += quant_2p_2N1(pos2, pos4, N) << (2*N); */
        index = L_add(index, L_shl(quant_2p_2N1(pos2, pos4, N), shl(N, 1)));
    } else
    {
        index = quant_2p_2N1(pos2, pos3, sub(N, 1));
        /* index += (pos2 & nb_pos) << N; */
        index = L_add(index, L_shl(L_deposit_l((Word16) (pos2 & nb_pos)), N));  logic16();
        /* index += quant_2p_2N1(pos1, pos4, N) << (2*N); */
        index = L_add(index, L_shl(quant_2p_2N1(pos1, pos4, N), shl(N, 1)));
    }
    return (index);
}

void dec_4p_4N1(Word32 index, Word16 N, Word16 offset, Word16 pos[])
{
    Word16 j, tmp;
    Word32 mask, idx;

    /*-------------------------------------------------------*
     * Decode 4 pulses with 4*N+1 bits:                      *
     *-------------------------------------------------------*/
    tmp = sub(shl(N, 1), 1);               /* mask = ((1<<((2*N)-1))-1); */
    mask = L_sub(L_shl(1L, tmp), 1L);
    idx = index & mask;                    logic16();
    j = offset;                            move16();
    tmp = sub(shl(N, 1), 1);

    test();logic16();
    if ((L_shr(index, tmp) & 1L) != 0L)
    {                                      /* (((index >> ((2*N)-1)) & 1) == 1) */
        j = add(j, shl(1, sub(N, 1)));     /* j += (1<<(N-1)); */
    }
    dec_2p_2N1(idx, (Word16) (N - 1), j, pos);


    tmp = add(shl(N, 1), 1);               /* mask = ((1<<((2*N)+1))-1); */
    mask = L_sub(L_shl(1L, tmp), 1L);
    idx = L_shr(index, shl(N, 1)) & mask;  logic16();/* idx = (index >> (2*N)) & mask; */
    dec_2p_2N1(idx, N, offset, pos + 2);   move16();  /* dec_2p_2N1(idx, N, offset, pos+2); */

    return;
}


Word32 quant_4p_4N(                        /* (o) return 4*N bits             */
     Word16 pos[],                         /* (i) position of the pulse 1..4  */
     Word16 N)                             /* (i) number of bits for position */
{
    Word16 i, j, k, nb_pos, mask, n_1, tmp;
    Word16 posA[4], posB[4];
    Word32 index;

    n_1 = (Word16) (N - 1);                move16();
    nb_pos = shl(1, n_1);                  /* nb_pos = (1<<n_1); */
    mask = sub(shl(1, N), 1);              /* mask = ((1<<N)-1); */