v90.c

Linmodem is soft modem source code for embedded system

/* 
 * Implementation of the V90 modulation/demodulation
 * 
 * Copyright (c) 1999,2000 Fabrice Bellard.
 *
 * This code is released under the GNU General Public License version
 * 2. Please read the file COPYING to know the exact terms of the
 * license.
 * 
 * This implementation is totally clean room. It was written by
 * reading the V90 specification and by using basic signal processing
 * knowledge.  
 */

#include "lm.h"
#include "v90priv.h"
#include "v34priv.h"

#define DEBUG

/* Compute the average power of a given constellation :
   nothing complicated here, except that each ucode must be counted
   correctly. We took the algorithm of the spec. It could be very
   optimized and simplified.
 */
int compute_power(V90EncodeState *s)
{
    int m,v,i,j;
    u64 p,n,r[6],k[6],a;

    n = ((u64)1 << s->K) - 1;
    for(i=0;i<6;i++) {
        r[i] = n;
        k[i] = n % s->M[i];
        n = n / s->M[i];
    }

    p = 0;
    a = 1;
    for(i=0;i<6;i++) {
        m = s->M[i];
        for(j=0;j<m;j++) {
            if (j < k[i]) {
                n = a * (r[i+1] + 1);
            } else if (j == k[i]) {
                n = ((u64)1 << s->K) - a * (r[i] - r[i+1]);
            } else {
                n = a * r[i+1];
            }
            v = s->ucode_to_linear[s->m_to_ucode[i][j]];
            p += v * v * n;
        }
        a = a * m;
    }
    return p / ((u64) 6 << s->K);
}

static const int sign_op[4] = { 0, 0x55, 0xff, 0xaa };

/* Select the best method for the current shaping frame
   We find the the shortest path in a treillis of depth ld
 */
static void select_best_signs(V90EncodeState *s, int sp_frame_size, int pp)
{
    int ucode_ptr, depth, i;
    int state, lstate, lstate_min;
    int t_min, x_min, y_min, v_min, w_min, sg;
    int x1, y1, v1, w1, x, y, v, w;
    u8  mem_l[2][TREILLIS_MAX_DEPTH+2], 
        mem_t[2][TREILLIS_MAX_DEPTH+2];
    s16 mem_x[2][TREILLIS_MAX_DEPTH+2], 
        mem_y[2][TREILLIS_MAX_DEPTH+2], 
        mem_v[2][TREILLIS_MAX_DEPTH+2], 
        mem_w[2][TREILLIS_MAX_DEPTH+2];
    
    /* save new pp signs */
    s->pp[s->ucode_ptr] = pp;

    /* Find best path starting at state Q */

    ucode_ptr = (s->ucode_ptr - (sp_frame_size * s->ld)) & (UCODE_BUF_SIZE-1);

    /* fill the treillis memory */
    mem_t[s->Q][0] = s->t;
    mem_x[s->Q][0] = s->x;
    mem_y[s->Q][0] = s->y;
    mem_v[s->Q][0] = s->v;
    mem_w[s->Q][0] = 0;

    for(depth = 0; depth <= s->ld; depth++) {
        
        for(state = 0; state < 2; state++) {
            
            w_min = 0x7fffffff;
            lstate_min = t_min = x_min = y_min = v_min = 0;
            
            /* we select the current state at the beginning of the treillis */
            if (depth == 0) 
                lstate = s->Q;
            else 
                lstate = 0;

            do {
                /* compute the signs */
                sg = mem_t[lstate][depth] ^ s->pp[ucode_ptr];
                /* apply the bit invertion method */
                sg ^= sign_op[(state << 1) | lstate];
                
                x1 = mem_x[lstate][depth];
                y1 = mem_y[lstate][depth];
                v1 = mem_v[lstate][depth];
                w1 = mem_w[lstate][depth];
                
                /* apply the spectral shaping filter to the frame */
                for(i=0;i<sp_frame_size;i++) {
                    x = s->ucode_to_linear[s->ucode[(ucode_ptr + i) & (UCODE_BUF_SIZE-1)]];
                    if (((sg >> i) & 1) == 0)
                        x = -x;
                    
                    y = x - ((s->b1 * x1 + s->a1 * y1) >> 6);
                    v = y - ((s->b2 * y1 + s->a2 * v1) >> 6);
                    w = ((v * v) >> 4) + w1;
                    
                    x1 = x;
                    y1 = y;
                    v1 = v;
                    w1 = w;
                }
                
                /* best transition ? */
                if (w1 < w_min) {
                    t_min = sg;
                    x_min = x1;
                    y_min = y1;
                    v_min = v1;
                    w_min = w1;
                    lstate_min = lstate;
                }
                lstate++;
            } while (lstate < 2 && depth != 0);
        
            /* selects the best transition */
            mem_t[state][depth+1] = t_min;
            mem_x[state][depth+1] = x_min;
            mem_y[state][depth+1] = y_min;
            mem_v[state][depth+1] = v_min;
            mem_w[state][depth+1] = w_min;
            mem_l[state][depth+1] = lstate_min;
        }

        ucode_ptr = (ucode_ptr + sp_frame_size) & (UCODE_BUF_SIZE-1);
    }

    /* now select the best next state by recursing thru the treillis */
    if (mem_w[1][s->ld + 1] < mem_w[0][s->ld + 1])
        state = 1;
    else
        state = 0;

    for(depth=s->ld;depth>0;depth--)
        state = mem_l[state][depth];
    
    s->Q = state;

    /* update current values */
    s->t = mem_t[state][1];
    s->x = mem_x[state][1];
    s->y = mem_y[state][1];
    s->v = mem_v[state][1];

    s->ucode_ptr = (s->ucode_ptr + sp_frame_size) & (UCODE_BUF_SIZE - 1);
}

/*
 * converts (S+K) data bits to 6 a/u law values (note: we output
 * linear values which may be converted back to u/a law). A delay of
 * (ld * frame_size) is introduced due to the shaping treillis.  
 */
static void v90_encode_mapping_frame(V90EncodeState *s, s16 *samples, u8 *data)
{
    int k, l, i, j, frame_size, nb_frames, p, signs;
    u64 v;
    int pv[3];

    /* modulo mapping to ucodes */
    v = 0;
    for(i=0;i<s->K;i++) v |= (data[i+s->S] << i);
    for(i=0;i<6;i++) {
        k = v % s->M[i];
        v /= s->M[i];
        /* compute the corresponding ucode */
        s->ucode[(s->ucode_ptr + i) & (UCODE_BUF_SIZE - 1)] = s->m_to_ucode[i][k];
    }
    
    /* computation of the sign of the ucodes */
    switch(s->S) {
    case 6:
    default:
        /* no redundancy & spectral shaping */
        l = s->last_sign;
        signs = 0;
        for(i=0;i<6;i++) {
            l = data[i] ^ l;
            signs |= (l << i);
        }
        s->last_sign = l;
        s->ucode_ptr = (s->ucode_ptr + 6) & (UCODE_BUF_SIZE - 1);
        frame_size = 6;
        goto skip_spectral_shaping;

    case 5:
        /* 1 bit of redundancy */
        {
            int pp1, pp3, pp5;
            
            pp1 = data[0] ^ s->last_sign;
            pp3 = data[2] ^ pp1;
            pp5 = data[4] ^ pp3;
            s->last_sign = pp5;
            
            pv[0] = (pp1 << 1) | (data[1] << 2) | (pp3 << 3) |
                (data[3] << 4) | (pp5 << 5);

            frame_size = 6;
        }
        break;
    case 4:
        /* 2 bits of redundancy */
        {
            int pp1, pp4;

            pp1 = data[0] ^ s->last_sign;
            pp4 = data[2] ^ pp1;

            s->last_sign = pp4;

            pv[0] = (pp1 << 1) | (data[1] << 2);
            pv[1] = (pp4 << 1) | (data[3] << 2);
            
            frame_size = 3;
        }
        break;

    case 3:
        /* 3 bits of redundancy */
        {
            int pp1, pp3, pp5;

            pp1 = data[0] ^ s->last_sign;
            pp3 = data[1] ^ pp1;
            pp5 = data[2] ^ pp3;
            
            s->last_sign = pp5;

            pv[0] = (pp1 << 1);
            pv[1] = (pp3 << 1);
            pv[2] = (pp5 << 1);

            frame_size = 2;
        }
        break;
    }

    /* select the best signs for each frame (with a delay of ld) */
    nb_frames = 6-s->S;
    signs = 0;
    for(i=0,j=0; i<nb_frames; i++, j+=frame_size) {
        select_best_signs(s, frame_size, pv[i]);
        l = s->t & ((1 << frame_size) - 1);
        signs |= (l << j);
    }

 skip_spectral_shaping:

    /* now the signs are computed, we can compute the pcm values from
       the ucodes. It may be faster to use the convertion already done
       for spectral shaping */
    p = (s->ucode_ptr - 6 - (frame_size * s->ld)) & (UCODE_BUF_SIZE - 1);
    for(i=0;i<6;i++) {
        int x;
        x = s->ucode_to_linear[s->ucode[p]];
        if (((signs >> i) & 1) == 0)
            x = -x;

        samples[i] = x;
        p = (p + 1) & (UCODE_BUF_SIZE - 1);
    }
}

/*
 * converts 6 a/u law samples to (S+K) data bits 
 */
static void v90_decode_mapping_frame(V90DecodeState *s, u8 *data, s16 *samples)
{
    s16 *tab;
    int d1, d2, i, j, l, val, v, m_min, m_max, m;
    u8 signs[6];
    u64 bitbuf;

    /* find signs & ucode */
    bitbuf = 0;
    for(j=5;j>=0;j--) {
        val = samples[j];
        if (val < 0) {
            val = - val;
            signs[j] = 0;
        } else {
            signs[j] = 1;
        }
        /* We look for the value closest to val with a binary search:
           see Knuth section 6.2 exercice 1. */
        tab = &s->m_to_linear[j][0];
        m_min = 0;
        m_max = s->M[j] - 1;
        while (m_min <= m_max) {
            m = (m_min + m_max) >> 1;
            v = tab[m];
            if (v == val)
                goto found;
            else if (val > v) {
                m_max = m - 1;
            } else {
                m_min = m + 1;
            }
        } 
        d1 = tab[m_max] - val;
        d2 = val - tab[m_min];
        if (d1 < d2) 
            m = m_max;
        else
            m = m_min;
    found:
        /* now we update the modulo value */
        bitbuf = bitbuf * s->M[j] + m;
    }

    /* output the K bits */
    for(i=0;i<s->K;i++) 
        data[i + s->S] = (bitbuf >> i) & 1;

    switch(s->S) {
    default:
    case 6:
        /* no redundant bit */
        l = s->last_sign;
        for(i=0;i<6;i++) {
            data[i] = l ^ signs[i];
            l = signs[i];
        }
        s->last_sign = l;
        break;
    case 5:
        /* 1 redundant bit */
        {
            int pp1, pp3, pp5, t0, pv0, Q1;

            t0 = 0;
            for(i=0;i<6;i++)
                t0 |= (signs[i] << i);
            
            pv0 = s->t ^ t0;
            Q1 = (pv0 & 1) ^ s->Q;
            pv0 ^= sign_op[s->Q | (Q1 << 1)] & 0x3f;
            s->t = t0;
            s->Q = Q1;
            
            /* extract the data bits */
            data[1] = (pv0 >> 2) & 1;
            data[3] = (pv0 >> 4) & 1;
            pp1 = ((pv0 >> 1) & 1);
            pp3 = ((pv0 >> 3) & 1);
            pp5 = ((pv0 >> 5) & 1);
            data[0] = pp1 ^ s->last_sign;
            data[2] = pp1 ^ pp3;
            data[4] = pp3 ^ pp5;
            s->last_sign = pp5;
        }
        break;
    case 4:
        /* 2 redundant bits */
        {
            int pp1, pp4, t0, t1, pv0, pv1, Q1, Q2;
            
            t0 = signs[0] | (signs[1] << 1) | (signs[2] << 2);
            t1 = signs[3] | (signs[4] << 1) | (signs[5] << 2);

            pv0 = s->t ^ t0;
            Q1 = (pv0 & 1) ^ s->Q;
            pv0 ^= sign_op[s->Q | (Q1 << 1)] & 7;

            pv1 = t0 ^ t1;
            Q2 = (pv1 & 1) ^ Q1;
            pv1 ^= sign_op[Q1 | (Q2 << 1)] & 7;

            s->t = t1;
            s->Q = Q2;
            
            data[1] = (pv0 >> 2) & 1;
            data[3] = (pv1 >> 2) & 1;
            pp1 = (pv0 >> 1) & 1;