retronetworking
/
linmodem
13
0
Fork 2
Code Issues Pull Requests Releases Wiki Activity
linmodem/v90.c

799 lines
20 KiB
C
Raw Blame History

/*
* 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;
pp4 = (pv1 >> 1) & 1;
data[0] = pp1 ^ s->last_sign;
data[2] = pp4 ^ pp1;
s->last_sign = pp4;
}
break;
case 3:
/* 3 redundant bits */
{
int pp1, pp3, pp5, t0, t1, t2, pv0, pv1, pv2, Q1, Q2, Q3;
t0 = signs[0] | (signs[1] << 1);
t1 = signs[2] | (signs[3] << 1);
t2 = signs[4] | (signs[5] << 1);
pv0 = s->t ^ t0;
Q1 = (pv0 & 1) ^ s->Q;
pv0 ^= sign_op[s->Q | (Q1 << 1)] & 3;
pv1 = t0 ^ t1;
Q2 = (pv1 & 1) ^ Q1;
pv1 ^= sign_op[Q1 | (Q2 << 1)] & 3;
pv2 = t1 ^ t2;
Q3 = (pv2 & 1) ^ Q2;
pv2 ^= sign_op[Q2 | (Q3 << 1)] & 3;
s->t = t2;
s->Q = Q3;
pp1 = (pv0 >> 1) & 1;
pp3 = (pv1 >> 1) & 1;
pp5 = (pv2 >> 1) & 1;
data[0] = pp1 ^ s->last_sign;
data[1] = pp3 ^ pp1;
data[2] = pp5 ^ pp3;
s->last_sign = pp5;
}
break;
}
}
static void compute_constellation(V90EncodeState *s,
u8 m_index[6], u8 ucode_used[6][128])
{
int i,j,k,m;
/* the ucode are taken from the bigger value down to the smaller value */
for(j=0;j<6;j++) {
k = m_index[j];
m = 0;
for(i=127;i>=0;i--) {
if (ucode_used[k][i]) {
s->m_to_ucode[j][m] = i;
m++;
}
}
s->M[j] = m;
}
#ifdef DEBUG
for(j=0;j<6;j++) {
printf("M[%d]: ", j);
for(i=0;i<s->M[j];i++) {
printf("%3d ", s->m_to_ucode[j][i]);
}
printf("\n");
}
#endif
}
void v90_encode_init(V90EncodeState *s)
{
}
void v90_decode_init(V90DecodeState *s)
{
int i,j,m;
/* some test values (You can modify them to test the
modulator/demodulator) */
for(i=0;i<6;i++) {
for(j=0;j<16;j++)
s->ucode_used[i][10 + j * 6] = 1;
}
s->K = 4 * 6;
s->S = 5;
s->alaw = 1;
s->ld = 2;
s->a1 = (int) (0.1 * 64);
s->a2 = (int) (0.2 * 64);
s->b1 = (int) (-0.1 * 64);
s->b2 = (int) (0.1 * 64);
/* we suppose that all other values are set to zero */
/* compute the decode tables */
if (s->alaw)
s->ucode_to_linear = v90_alaw_ucode_to_linear;
else
s->ucode_to_linear = v90_ulaw_ucode_to_linear;
for(j=0;j<6;j++) {
m = 0;
for(i=127;i>=0;i--) {
if (s->ucode_used[j][i]) {
s->m_to_linear[j][m] = s->ucode_to_linear[i];
m++;
}
}
s->M[j] = m;
}
}
static u8 test_buf[1000];
/* send a CP frame (analog modem) */
static void v90_send_CP(V90DecodeState *s, int is_CP, int ack)
{
u8 *buf, *p;
int i, crc, drn;
buf = test_buf;
p = buf;
put_bits(&p, 17, 0x1ffff); /* frame sync */
put_bits(&p, 1, 0); /* start bit */
put_bits(&p, 1, 0); /* reserved */
put_bits(&p, 1, is_CP); /* 0=CPt 1=CP frame */
drn = s->K + s->S;
if (is_CP)
drn -= 20;
else
drn -= 8;
put_bits(&p, 5, drn); /* drn: speed */
put_bits(&p, 5, 0); /* reserved */
put_bits(&p, 1, 0); /* 1 if silence asked */
put_bits(&p, 2, 6 - s->S); /* Sr */
put_bits(&p, 1, ack); /* ack */
put_bits(&p, 1, 0); /* start bit */
put_bits(&p, 1, s->alaw); /* u/a law selection */
for(i=0;i<13;i++) {
put_bits(&p, 1, 1); /* speed (i+2) * 2400 supported (V34 part) */
}
put_bits(&p, 2, s->ld);
put_bits(&p, 1, 0); /* start bit */
put_bits(&p, 16, 0); /* RMS value for TRN1d */
put_bits(&p, 1, 0); /* start bit */
put_bits(&p, 8, s->a1 & 0xff); /* spectral shaping parameters */
put_bits(&p, 8, s->a2 & 0xff); /* spectral shaping parameters */
put_bits(&p, 1, 0); /* start bit */
put_bits(&p, 8, s->b1 & 0xff); /* spectral shaping parameters */
put_bits(&p, 8, s->b2 & 0xff); /* spectral shaping parameters */
put_bits(&p, 1, 0); /* start bit */
for(i=0;i<6;i++) {
if (i == 4)
put_bits(&p, 1, 0); /* start bit */
put_bits(&p, 4, 0); /* modulation index */
}
put_bits(&p, 1, 0); /* different different tx - D/A constellations ? */
put_bits(&p, 7, 0); /* reserved */
/* transmit the constellation */
for(i=0;i<128;i++) {
if ((i & 15) == 0)
put_bits(&p, 1, 0); /* start bit */
put_bits(&p, 1, s->ucode_used[0][i]);
}
/* CRC */
put_bits(&p, 1, 0); /* start bit */
crc = calc_crc(buf + 17, p - (buf+17));
put_bits(&p, 16, crc);
put_bits(&p, 3, 0); /* fill */
printf("CP size= %d\n", p - buf);
}
static int get_bit(u8 **pp)
{
u8 *p;
int v;
p = *pp;
v = *p++;
*pp = p;
return v;
}
static int get_bits(u8 *p, int n)
{
int i, v;
v = 0;
for(i=n-1;i>=0;i--) {
v |= *p++ << i;
}
return v;
}
/* parse the CP packet & compute the V90 parameters */
static void v90_parse_CP(V90EncodeState *s, u8 *buf)
{
u8 m_index[6];
u8 ucode_used[7][128];
int drn, i, j, k, nb_constellations;
/* now the whole packet is can be read */
s->S = 6 - get_bits(buf + 31, 2);
s->alaw = get_bits(buf + 35, 1);
s->ld = get_bits(buf + 49, 2);
s->a1 = (s8) get_bits(buf + 69, 8);
s->a2 = (s8) get_bits(buf + 77, 8);
s->b1 = (s8) get_bits(buf + 86, 8);
s->b2 = (s8) get_bits(buf + 94, 8);
for(i=0;i<4;i++)
m_index[i] = get_bits(buf + 103 + i * 4, 4);
for(i=0;i<2;i++)
m_index[4 + i] = get_bits(buf + 120 + i * 4, 4);
nb_constellations = 0;
for(i=0;i<6;i++) {
if (m_index[i] > nb_constellations) nb_constellations = m_index[i];
}
nb_constellations++;
if (buf[128])
nb_constellations++;
for(i=0;i<nb_constellations;i++) {
for(j=0;j<128;j++) {
k = i * 128 + j;
ucode_used[i][j] = buf[137 + (17 * (k >> 4)) + (k & 15)];
}
}
/* compute K */
drn = get_bits(buf + 20, 5);
if (buf[19])
drn += 20;
else
drn += 8;
s->K = drn - s->S;
if (s->alaw)
s->ucode_to_linear = v90_alaw_ucode_to_linear;
else
s->ucode_to_linear = v90_ulaw_ucode_to_linear;
printf("V90_received_CP:\n");
compute_constellation(s, m_index, ucode_used);
printf("S=%d K=%d R=%d alaw=%d ld=%d a1=%d a2=%d b1=%d b2=%d\n",
s->S, s->K, ((s->S + s->K) * 8000) / 6,
s->alaw, s->ld, s->a1, s->a2, s->b1, s->b2);
}
/* received & parse the CP packet */
static void v90_receive_CP(V90EncodeState *s)
{
u8 buf[1024], *p, *q;
int b, i, frame_index, one_count, frame_count, nb_constellations;
int crc1;
u8 m_index[6];
p = test_buf;
wait_sync:
one_count = 0;
while (get_bit(&p)) {
one_count++;
}
if (one_count != 17)
goto wait_sync;
#ifdef DEBUG
printf("got CP sync\n");
#endif
frame_index = 0;
frame_count = 8;
crc1 = 1;
q = buf + 17;
while (frame_index < frame_count) {
printf("%2d: ", frame_index);
*q++ = 0;
for(i=0;i<16;i++) {
b = get_bit(&p);
*q++ = b;
printf("%d", b);
}
printf("\n");
if (frame_index == 6) {
/* compute the number of constellation to read */
for(i=0;i<4;i++) {
m_index[i] = get_bits(buf + 103 + i * 4, 4);
}
for(i=0;i<2;i++) {
m_index[4 + i] = get_bits(buf + 120 + i * 4, 4);
}
nb_constellations = 0;
for(i=0;i<6;i++) {
if (m_index[i] > nb_constellations) nb_constellations = m_index[i];
}
nb_constellations++;
if (buf[128])
nb_constellations++;
frame_count += 8 * nb_constellations;
}
if (frame_index == (frame_count - 1)) {
/* check the crc (it must be zero because we include the crc itself) */
crc1 = calc_crc(buf + 17, q - (buf+17));
}
if (get_bit(&p) != 0) {
printf("start bit expected\n");
goto wait_sync;
}
frame_index++;
}
if (crc1 != 0)
goto wait_sync;
v90_parse_CP(s, buf);
}
/* simple test of V90 algebraic computations */
/* Note: if ld != 0 and 3 <= S <= 4, the delay introduced with data[][]
is not correct */
void V90_test(void)
{
int i,j,n,l;
V90EncodeState v90_enc;
V90DecodeState v90_dec;
u8 data[5][48], data1[48];
s16 samples[6];
/* init modem state */
memset(&v90_enc, 0, sizeof(v90_enc));
v90_encode_init(&v90_enc);
memset(&v90_dec, 0, sizeof(v90_dec));
v90_decode_init(&v90_dec);
/* send the CP sequence which contains the modulation parameters */
v90_send_CP(&v90_dec, 1, 0);
/* "receive" it ! */
v90_receive_CP(&v90_enc);
/* number of data bits per mapping frame */
n = v90_enc.S + v90_enc.K;
/* transmit & receive 1000 mapping frames */
memset(data, 0, sizeof(data));
l = 0;
for(i=0;i<1000;i++) {
for(j=0;j<n;j++) data[l][j] = random() & 1;
v90_encode_mapping_frame(&v90_enc, samples, data[l]);
// for(j=0;j<6;j++) samples[j] += (random() % 32) - 16;
printf("%4d: ", i);
for(j=0;j<6;j++) printf("%6d,", samples[j]);
printf("\n");
v90_decode_mapping_frame(&v90_dec, data1, samples);
l = (l + 1) % (v90_dec.ld+1);
for(j=0;j<n;j++) {
if (data[l][j] != data1[j]) {
printf("error mapping frame=%d bit=%d\n", i, j);
}
}
}
}
View Git Blame Copy Permalink