g727.c

Reference Implementation of G.711 standard and other voice codecs

    else
        assert(0);

    prodm = ((Int32)difm*al) >> 6;

    if (difs == 0)
        prod = prodm;
    else if (difs == 1)
        prod = (16384 - prodm) & 16383;
    else
        assert(0);

    return ((st->yl >> 6) + prod) & 8191;
}


/*---------- Adaptation Speed Control (G.727 6.2.6) ----------*/

void
g727_asc_reset(g727_asc_state *st)
{
    st->dms = 0;
    st->dml = 0;
    st->ap = 0;
}


Int8
g727_get_al(g727_asc_state *st)
{
    return g727_lima(st);
}


void
g727_asc_transit(g727_asc_state *st, Int8 in, Int16 y, Int8 tr, Int8 tdp,
                                                                short rate)
{
    Int8 fi = g727_functf(in, rate);
    Int16 dmsp = g727_filta(st, fi);
    Int16 dmlp = g727_filtb(st, fi);
    Int16 apr = g727_triga(tr, g727_filtc(st, g727_subtc(dmsp, dmlp, tdp, y)));

    st->dms = dmsp;
    st->dml = dmlp;
    st->ap = apr;
}


Int16
g727_filta(g727_asc_state *st, Int8 fi)
{
    Int16 dif = ((fi << 9) + 8192 - st->dms) & 8191;
    Int8 difs = dif >> 12;
    Int16 difsx;

    if (difs == 0)
        difsx = dif >> 5;
    else if (difs == 1)
        difsx = (dif >> 5) + 3840;
    else
        assert(0);

    return (difsx + st->dms) & 4095;
}


Int16
g727_filtb(g727_asc_state *st, Int8 fi)
{
    Int16 dif = ((fi << 11) + 32768 - st->dml) & 32767;
    Int8 difs = dif >> 14;
    Int16 difsx;

    if (difs == 0)
        difsx = dif >> 7;
    else if (difs == 1)
        difsx = (dif >> 7) + 16128;
    else
        assert(0);

    return (difsx + st->dml) & 16383;
}


Int16
g727_filtc(g727_asc_state *st, Int8 ax)
{
    Int16 dif = ((ax << 9) + 2048 - st->ap) & 2047;
    Int8 difs = dif >> 10;
    Int16 difsx;

    if (difs == 0)
        difsx = dif >> 4;
    else if (difs == 1)
        difsx = (dif >> 4) + 896;
    else
        assert(0);

    return (difsx + st->ap) & 1023;
}


Int8
g727_functf(Int8 in, short rate)
{
    Int8 is;
    Int8 im;
    Int8 fi;

    switch (rate) {
        case 4:
            is = in >> 3;

            if (is == 0)
                im = in & 7;
            else if (is == 1)
                im = (15 - in) & 7;
            else
                assert(0);

            fi = g727_fiTable32[im];
            break;

        case 3:
            is = in >> 2;

            if (is == 0)
                im = in & 3;
            else if (is == 1)
                im = (7 - in) & 3;
            else
                assert(0);

            fi = g727_fiTable24[im];
            break;

        case 2:
            is = in >> 1;

            if (is == 0)
                im = in & 1;
            else if (is == 1)
                im = (3 - in) & 1;
            else
                assert(0);

            fi = g727_fiTable16[im];
            break;

        default:
            assert(0);
    }

    return fi;
}


Int8
g727_lima(g727_asc_state *st)
{
    return (st->ap >= 256) ? 64 : (st->ap >> 2);
}


Int8
g727_subtc(Int16 dmsp, Int16 dmlp, Int8 tdp, Int16 y)
{
    Int16 dif = ((dmsp << 2) + 32768 - dmlp) & 32767;
    Int8 difs = dif >> 14;
    Int16 difm;
    Int16 dthr;

    if (difs == 0)
        difm = dif;
    else if (difs == 1)
        difm = (32768 - dif) & 16383;
    else
        assert(0);

    dthr = dmlp >> 3;

    return (y >= 1536 && difm < dthr && tdp == 0) ? 0 : 1;
}


Int16
g727_triga(Int8 tr, Int16 app)
{
    Int16 apr;

    if (tr == 0)
        apr = app;
    else if (tr == 1)
        apr = 256;
    else
        assert(0);

    return apr;
}


/*---------- Adaptive Predictor and Reconstructed  ----------*/
/*---------- Signal Calculator (G.727 6.2.7) ----------------*/

void
g727_aprsc_reset(g727_aprsc_state *st)
{
    st->pk1 = 0;
    st->pk2 = 0;
    st->sr1 = 32;
    st->sr2 = 32;
    st->dq1 = 32;
    st->dq2 = 32;
    st->dq3 = 32;
    st->dq4 = 32;
    st->dq5 = 32;
    st->dq6 = 32;
    st->b1 = 0;
    st->b2 = 0;
    st->b3 = 0;
    st->b4 = 0;
    st->b5 = 0;
    st->b6 = 0;
    st->a1 = 0;
    st->a2 = 0;
}


Int16
g727_get_sr(Int16 dq, Int16 se)
{
    return g727_addb(dq, se);
}


void
g727_get_se_sez(g727_aprsc_state *st, Int16 *se, Int16 *sez)
{
    g727_accum(se, sez, g727_fmult(st->a1, st->sr1),
                        g727_fmult(st->a2, st->sr2),
                        g727_fmult(st->b1, st->dq1),
                        g727_fmult(st->b2, st->dq2),
                        g727_fmult(st->b3, st->dq3),
                        g727_fmult(st->b4, st->dq4),
                        g727_fmult(st->b5, st->dq5),
                        g727_fmult(st->b6, st->dq6));
}


Int16
g727_get_a2p(g727_aprsc_state *st, Int16 dq, Int16 sez, Int8 *pk0,
                                                        Int8 *sigpk)
{
    g727_addc(pk0, sigpk, dq, sez);

    return g727_limc(g727_upa2(st, *pk0, *sigpk));
}


void
g727_aprsc_transit(g727_aprsc_state *st, Int16 dq, Int8 tr, Int16 sr,
                                                            Int16 a2p,
                                                            Int8 pk0,
                                                            Int8 sigpk)
{
    Int16 sr0;
    Int16 dq0;
    Int16 a1r;
    Int16 a2r;
    Int16 b1r;
    Int16 b2r;
    Int16 b3r;
    Int16 b4r;
    Int16 b5r;
    Int16 b6r;

    sr0 = g727_floatb(sr);

    dq0 = g727_floata(dq);

    a1r = g727_aprsc_trigb(tr, g727_limd(g727_upa1(st, pk0, sigpk), a2p));
    a2r = g727_aprsc_trigb(tr, a2p);

    b1r = g727_aprsc_trigb(tr, g727_upb(g727_xor(st->dq1, dq), st->b1, dq));
    b2r = g727_aprsc_trigb(tr, g727_upb(g727_xor(st->dq2, dq), st->b2, dq));
    b3r = g727_aprsc_trigb(tr, g727_upb(g727_xor(st->dq3, dq), st->b3, dq));
    b4r = g727_aprsc_trigb(tr, g727_upb(g727_xor(st->dq4, dq), st->b4, dq));
    b5r = g727_aprsc_trigb(tr, g727_upb(g727_xor(st->dq5, dq), st->b5, dq));
    b6r = g727_aprsc_trigb(tr, g727_upb(g727_xor(st->dq6, dq), st->b6, dq));

    st->pk2 = st->pk1;
    st->pk1 = pk0;

    st->sr2 = st->sr1;
    st->sr1 = sr0;

    st->a1 = a1r;
    st->a2 = a2r;

    st->b1 = b1r;
    st->b2 = b2r;
    st->b3 = b3r;
    st->b4 = b4r;
    st->b5 = b5r;
    st->b6 = b6r;

    st->dq6 = st->dq5;
    st->dq5 = st->dq4;
    st->dq4 = st->dq3;
    st->dq3 = st->dq2;
    st->dq2 = st->dq1;
    st->dq1 = dq0;
}


void
g727_accum(Int16 *se, Int16 *sez, Int16 wa1, Int16 wa2, Int16 wb1,
                                                        Int16 wb2,
                                                        Int16 wb3,
                                                        Int16 wb4,
                                                        Int16 wb5,
                                                        Int16 wb6)
{
    Int16 sezi = (((((((((wb1 + wb2) & 65535) + wb3) & 65535) +
                 wb4) & 65535) + wb5) & 65535) + wb6) & 65535;
    Int16 sei = (((sezi + wa2) & 65535) + wa1) & 65535;

    *sez = sezi >> 1;
    *se = sei >> 1;
}


Int16
g727_addb(Int16 dq, Int16 se)
{
    Int8 dqs = dq >> 14;
    Int16 dqi;
    Int8 ses;
    Int16 sei;

    if (dqs == 0)
        dqi = dq;
    else if (dqs == 1)
        dqi = (65536 - (dq & 16383)) & 65535;
    else
        assert(0);

    ses = se >> 14;

    if (ses == 0)
        sei = se;
    else if (ses == 1)
        sei = (1U << 15) + se;
    else
        assert(0);

    return (dqi + sei) & 65535;
}


void
g727_addc(Int8 *pk0, Int8 *sigpk, Int16 dq, Int16 sez)
{
    Int8 dqs = dq >> 14;
    Int16 dqi;
    Int8 sezs;
    Int16 sezi;
    Int16 dqsez;

    if (dqs == 0)
        dqi = dq;
    else if (dqs == 1)
        dqi = (65536 - (dq & 16383)) & 65535;
    else
        assert(0);

    sezs = sez >> 14;

    if (sezs == 0)
        sezi = sez;
    else if (sezs == 1)
        sezi = (1U << 15) + sez;
    else
        assert(0);

    dqsez = (dqi + sezi) & 65535;
    *pk0 = dqsez >> 15;
    *sigpk = (dqsez == 0) ? 1 : 0;
}


Int8
g727_makexp(Int16 mag)
{
    if (16384 <= mag)
        return 15;
    if (8192 <= mag)
        return 14;
    if (4096 <= mag)
        return 13;
    if (2048 <= mag)
        return 12;
    if (1024 <= mag)
        return 11;
    if (512 <= mag)
        return 10;
    if (256 <= mag)
        return 9;
    if (128 <= mag)
        return 8;
    if (64 <= mag)
        return 7;
    if (32 <= mag)
        return 6;
    if (16 <= mag)
        return 5;
    if (8 <= mag)
        return 4;
    if (4 <= mag)
        return 3;
    if (2 <= mag)
        return 2;

    return mag;
}


Int16
g727_floata(Int16 dq)
{
    Int8 dqs = dq >> 14;
    Int16 mag = dq & 16383;
    Int8 exp = g727_makexp(mag);
    Int8 mant = (mag == 0) ? (1 << 5) : (((Int32)mag << 6) >> exp);

    return (dqs << 10) + (exp << 6) + mant;
}


Int16
g727_floatb(Int16 sr)
{
    Int8 srs = sr >> 15;
    Int16 mag;
    Int8 exp;
    Int8 mant;

    if (srs == 0)
        mag = sr;
    else if (srs == 1)
        mag = (65536 - sr) & 32767;
    else
        assert(0);

    exp = g727_makexp(mag);
    mant = (mag == 0) ? (1 << 5) : (((Int32)mag << 6) >> exp);

    return (srs << 10) + (exp << 6) + mant;
}


Int16
g727_fmult(Int16 an, Int16 srn)
{
    Int8 ans = an >> 15;
    Int16 anmag;
    Int8 anexp;
    Int8 anmant;
    Int8 srns;
    Int8 srnexp;
    Int8 srnmant;
    Int8 wans;
    Int8 wanexp;
    Int8 wanmant;
    Int16 wanmag;
    Int16 wan;

    if (ans == 0)
        anmag = an >> 2;
    else if (ans == 1)
        anmag = (16384 - (an >> 2)) & 8191;
    else
        assert(0);

    anexp = g727_makexp(anmag);
    anmant = (anmag == 0) ? (1 << 5) : (((Int32)anmag << 6) >> anexp);

    srns = srn >> 10;
    srnexp = (srn >> 6) & 15;
    srnmant = srn & 63;

    wans = srns ^ ans;
    wanexp = srnexp + anexp;
    wanmant = ((srnmant*anmant) + 48) >> 4;
    wanmag = (wanexp <= 26 ) ?
             (((Int16)wanmant << 7) >> (26 - wanexp)) :
             ((((Int16)wanmant << 7) << (wanexp - 26)) & 32767);

    if (wans == 0)
        wan = wanmag;
    else if (wans == 1)
        wan = (65536 - wanmag) & 65535;
    else
        assert(0);

    return wan;
}


Int16
g727_limc(Int16 a2t)
{
    Int16 a2ul = 12288;
    Int16 a2ll = 53248;

    return (32768 <= a2t && a2t <= a2ll) ? a2ll :
           ((a2ul <= a2t && a2t <= 32767) ? a2ul : a2t);
}


Int16
g727_limd(Int16 a1t, Int16 a2p)