retronetworking
/
linmodem
13
0
Fork 1
Code Issues Pull Requests Releases Wiki Activity
linmodem/v34.c

2312 lines
55 KiB
C
Raw Permalink Blame History

/*
* Implementation of the V34 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 V34 specification and by using basic signal processing
* knowledge.
*/
#include "lm.h"
#include "v34priv.h"
#define DEBUG
void print_bits(int val, int n)
{
int i;
for(i=n-1;i>=0;i--) {
putchar('0' + ((val >> i) & 1));
}
}
void print_bit_vector(char *str, u8 *tab, int n)
{
int i;
printf("%s[%d]= ", str, n);
for(i=n-1;i>=0;i--) putchar(tab[i] + '0');
printf("\n");
}
static void agc_init(V34DSPState *s);
static void baseband_decode(V34DSPState *s, int si, int sq);
static void put_sym(V34DSPState *s, int si, int sq);
/* divide the bit sequence by poly */
#define SCRAMBLER_DEG 23
#define V34_GPC (1 | (1 << (23-18)))
#define V34_GPA (1 | (1 << (23-5)))
int scramble_bit(V34DSPState *s, int b, int poly)
{
int b1, reg;
reg = s->scrambler_reg;
b1 = (reg >> (SCRAMBLER_DEG-1)) ^ b;
reg = (reg << 1) & ((1 << SCRAMBLER_DEG) - 1);
if (b1)
reg ^= poly;
s->scrambler_reg = reg;
return b1;
}
int unscramble_bit(V34DSPState *s, int b, int poly)
{
int b1, reg;
reg = s->scrambler_reg;
b1 = (reg >> (SCRAMBLER_DEG-1)) ^ b;
reg = (reg << 1) & ((1 << SCRAMBLER_DEG) - 1);
if (b)
reg ^= poly;
s->scrambler_reg = reg;
return b1;
}
/* build the constellation (no need to store it completely, we are lazy!) */
static int constellation_cmp(const void *a_ptr, const void *b_ptr)
{
int x1,y1,x2,y2,d;
x1= ((s8 *)a_ptr)[0];
y1= ((s8 *)a_ptr)[1];
x2= ((s8 *)b_ptr)[0];
y2= ((s8 *)b_ptr)[1];
d = (x1 * x1 + y1 * y1) - (x2 * x2 + y2 * y2) ;
if (d != 0)
return d;
else
return y2 - y1;
}
#define rotate_clockwise(x, y, x1, y1, z)\
{\
switch(z) {\
case 0:\
x = x1;\
y = y1;\
break;\
case 1:\
x = -y1;\
y = x1;\
break;\
case 2:\
x = -x1;\
y = -y1;\
break;\
default:\
x = y1;\
y = -x1;\
break;\
}\
}
static void build_constellation(V34DSPState *s)
{
int x,y,i,j,k;
k = 0;
for(y=C_MIN; y<= C_MAX; y++) {
for(x=C_MIN; x<= C_MAX; x++) {
s->constellation[k][0] = 4*x+1;
s->constellation[k][1] = 4*y+1;
k++;
}
}
/* now sort the constellation */
qsort(s->constellation, C_MAX_SIZE, 2, constellation_cmp);
#if 0
for(i=0;i<L_MAX/4;i++) printf("%d: x=%d y=%d\n",
i, s->constellation[i][0], s->constellation[i][1]);
#endif
/* build the table for the decoder (not the best table, the corners
are not ok) */
memset(s->constellation_to_code, 0, sizeof(s->constellation_to_code));
for(j=0;j<4;j++) {
for(i=0;i<L_MAX/4;i++) {
int x1,y1;
x1 = s->constellation[i][0];
y1 = s->constellation[i][1];
rotate_clockwise(x, y, x1, y1, j);
x = (x + C_RADIUS) >> 1;
y = (y + C_RADIUS) >> 1;
s->constellation_to_code[x][y] = i | (j << 14);
}
}
}
/* index to ring utilities */
static inline int g2(V34DSPState *st, int p, int m)
{
if (p >= 0 && p <= 2*(m-1))
return m - abs(p-(m-1));
else {
return 0;
}
}
static inline int g4(V34DSPState *st, int p, int m)
{
int s,i;
s = 0;
if (p >= 0 && p <= 4*(m-1)) {
for(i=0;i<=p;i++) s += st->g2_tab[i] * st->g2_tab[p-i];
}
return s;
}
static inline int g8(V34DSPState *st, int p, int m)
{
int s,i;
s = 0;
if (p >= 0 && p <= 8*(m-1)) {
for(i=0;i<=p;i++) s += st->g4_tab[i] * st->g4_tab[p-i];
}
return s;
}
static void index_to_rings(V34DSPState *s, int ring[4][2], int r0)
{
int a,b,c,d,e,f,g,h,r1,r2,r3,r4,r5,tmp,m;
m = s->M;
a = -1;
r1 = 0;
for(;;) {
tmp = r0 - s->z8_tab[a+1];
if (tmp < 0) break;
r1 = tmp;
a++;
}
b = 0;
for(;;) {
tmp = r1 - s->g4_tab[b] * s->g4_tab[a-b];
if (tmp < 0) break;
r1 = tmp;
b++;
}
tmp = s->g4_tab[b];
r2 = r1 % tmp;
r3 = (r1 - r2) / tmp;
c = 0;
r4 = r2;
for(;;) {
tmp = r4 - s->g2_tab[c] * s->g2_tab[b-c];
if (tmp < 0) break;
r4 = tmp;
c++;
}
d = 0;
r5 = r3;
for(;;) {
tmp = r5 - s->g2_tab[d] * s->g2_tab[a-b-d];
if (tmp < 0) break;
r5 = tmp;
d++;
}
tmp = s->g2_tab[c];
e = r4 % tmp;
f = (r4 - e) / tmp;
tmp = s->g2_tab[d];
g = r5 % tmp;
h = (r5 - g) / tmp;
if (c < m) {
ring[0][0] = e;
ring[0][1] = c - ring[0][0];
} else {
ring[0][1] = m - 1 - e;
ring[0][0] = c - ring[0][1];
}
if ((b-c) < m) {
ring[1][0] = f;
ring[1][1] = b - c - ring[1][0];
} else {
ring[1][1] = m - 1 - f;
ring[1][0] = b - c - ring[1][1];
}
if (d < m) {
ring[2][0] = g;
ring[2][1] = d - ring[2][0];
} else {
ring[2][1] = m - 1 - g;
ring[2][0] = d - ring[2][1];
}
if ((a-b-d) < m) {
ring[3][0] = h;
ring[3][1] = a - b - d - ring[3][0];
} else {
ring[3][1] = m - 1 - h;
ring[3][0] = a - b - d - ring[3][1];
}
}
/* return the K bit index corresponding to the rings */
static int rings_to_index(V34DSPState *s, int ring[4][2])
{
int a,b,c,d,e,f,g,h,r0,r1,r2,r3,r4,r5,m,i;
m = s->M;
/* find back the parameters */
c = ring[0][0] + ring[0][1];
if (c < m) e = ring[0][0]; else e = m - 1 - ring[0][1];
b = ring[1][0] + ring[1][1];
if (b < m) f = ring[1][0]; else f = m - 1 - ring[1][1];
b += c;
d = ring[2][0] + ring[2][1];
if (d < m) g = ring[2][0]; else g = m - 1 - ring[2][1];
a = ring[3][0] + ring[3][1];
if (a < m) h = ring[3][0]; else h = m - 1 - ring[3][1];
a += b + d;
r5 = h * s->g2_tab[d] + g;
r4 = f * s->g2_tab[c] + e;
r3 = r5;
for(i=0;i<d;i++) r3 += s->g2_tab[i] * s->g2_tab[a-b-i];
r2 = r4;
for(i=0;i<c;i++) r2 += s->g2_tab[i] * s->g2_tab[b-i];
r1 = r3 * s->g4_tab[b] + r2;
for(i=0;i<b;i++) r1 += s->g4_tab[i] * s->g4_tab[a-i];
r0 = r1 + s->z8_tab[a];
return r0;
}
/* initialize the g2, g4, g8 & z8 tables */
static void build_rings(V34DSPState *s)
{
int n,i,m;
m = s->M;
n = 8*(m - 1) + 1;
for(i=0;i<n;i++) s->g2_tab[i] = g2(s,i,m);
for(i=0;i<n;i++) s->g4_tab[i] = g4(s,i,m);
for(i=0;i<n;i++) s->g8_tab[i] = g8(s,i,m);
s->z8_tab[0] = 0;
for(i=1;i<n;i++) {
s->z8_tab[i] = s->z8_tab[i-1] + s->g8_tab[i-1];
}
#if 0
{
int i;
for(i=0;i<=(1 << s->K);i++) {
int m[4][2],j, r1,r0;
r0 = random() % (1 << s->K);
index_to_rings(s, m, r0);
r1 = rings_to_index(s, m);
printf("%d:", r0);
for(j=0;j<4;j++) printf(" %d %d",m[j][0], m[j][1]);
printf("\n");
if (r0 != r1) {
printf("error r0=%d r1=%d\n" , r0, r1);
exit(1);
}
}
}
#endif
}
/* parameters for each symbol rate */
static u8 S_tab[6][8] = {
/* a, c, d1, e1, d2, e2, J, P */
{ 1, 1, 2, 3, 3, 4, 7, 12, }, /* S=2400 */
{ 8, 7, 3, 5, 2, 3, 8, 12, }, /* S=2743 */
{ 7, 6, 3, 5, 2, 3, 7, 14, }, /* S=2800 */
{ 5, 4, 3, 5, 2, 3, 7, 15, }, /* S=3000 */
{ 4, 3, 4, 7, 3, 5, 7, 16, }, /* S=3200 */
{10, 7, 4, 7, 4, 7, 8, 15, }, /* S=3429 */
};
/* this table depends on the sample rate. We have S=a1/c1 * V34_SAMPLE_RATE */
static u8 baud_tab[6][2] = {
/* a1, c1 */
{ 3, 10 },
{ 12, 35 },
{ 7, 20 },
{ 3, 8 },
{ 2, 5 },
{ 3, 7 },
};
static s16 *rc_filter[6] = {
v34_rc_10_filter,
v34_rc_35_filter,
v34_rc_20_filter,
v34_rc_8_filter,
v34_rc_5_filter,
v34_rc_7_filter,
};
static void build_tx_filter(V34DSPState *s)
{
/* sampled at every symbol */
s->tx_filter = rc_filter[s->S];
s->baud_incr = s->symbol_rate * (float)0x10000 / (float)V34_SAMPLE_RATE;
s->baud_phase = 4;
s->carrier_phase = 0;
s->carrier_incr = s->carrier_freq * (float)0x10000 / (float)V34_SAMPLE_RATE;
/* init TX fifo */
s->tx_filter_wsize = RC_FILTER_SIZE;
s->tx_buf_ptr = 0;
s->tx_outbuf_ptr = s->tx_filter_wsize;
s->tx_buf_size = 0;
}
float hilbert[156] =
{
/* alpha=0.000000 beta=0.000000 */
0.0000000000e+00,
1.0438059849e-03,
0.0000000000e+00,
1.1113855722e-03,
0.0000000000e+00,
1.2232369871e-03,
0.0000000000e+00,
1.3825587134e-03,
0.0000000000e+00,
1.5926453386e-03,
0.0000000000e+00,
1.8569074639e-03,
0.0000000000e+00,
2.1788971424e-03,
0.0000000000e+00,
2.5623400268e-03,
0.0000000000e+00,
3.0111756977e-03,
0.0000000000e+00,
3.5296080377e-03,
0.0000000000e+00,
4.1221680212e-03,
0.0000000000e+00,
4.7937919718e-03,
0.0000000000e+00,
5.5499192449e-03,
0.0000000000e+00,
6.3966145267e-03,
0.0000000000e+00,
7.3407216217e-03,
0.0000000000e+00,
8.3900579330e-03,
0.0000000000e+00,
9.5536620984e-03,
0.0000000000e+00,
1.0842111879e-02,
0.0000000000e+00,
1.2267936058e-02,
0.0000000000e+00,
1.3846153846e-02,
0.0000000000e+00,
1.5594989773e-02,
0.0000000000e+00,
1.7536833977e-02,
0.0000000000e+00,
1.9699551684e-02,
0.0000000000e+00,
2.2118299263e-02,
0.0000000000e+00,
2.4838091053e-02,
0.0000000000e+00,
2.7917505894e-02,
0.0000000000e+00,
3.1434171201e-02,
0.0000000000e+00,
3.5493106154e-02,
0.0000000000e+00,
4.0239829657e-02,
0.0000000000e+00,
4.5881743462e-02,
0.0000000000e+00,
5.2724604849e-02,
0.0000000000e+00,
6.1238171887e-02,
0.0000000000e+00,
7.2182437365e-02,
0.0000000000e+00,
8.6871563629e-02,
0.0000000000e+00,
1.0778971907e-01,
0.0000000000e+00,
1.4026261876e-01,
0.0000000000e+00,
1.9814073999e-01,
0.0000000000e+00,
3.3221536070e-01,
0.0000000000e+00,
9.9962693916e-01,
0.0000000000e+00,
-9.9962693916e-01,
-0.0000000000e+00,
-3.3221536070e-01,
-0.0000000000e+00,
-1.9814073999e-01,
-0.0000000000e+00,
-1.4026261876e-01,
-0.0000000000e+00,
-1.0778971907e-01,
-0.0000000000e+00,
-8.6871563629e-02,
-0.0000000000e+00,
-7.2182437365e-02,
-0.0000000000e+00,
-6.1238171887e-02,
-0.0000000000e+00,
-5.2724604849e-02,
-0.0000000000e+00,
-4.5881743462e-02,
-0.0000000000e+00,
-4.0239829657e-02,
-0.0000000000e+00,
-3.5493106154e-02,
-0.0000000000e+00,
-3.1434171201e-02,
-0.0000000000e+00,
-2.7917505894e-02,
-0.0000000000e+00,
-2.4838091053e-02,
-0.0000000000e+00,
-2.2118299263e-02,
-0.0000000000e+00,
-1.9699551684e-02,
-0.0000000000e+00,
-1.7536833977e-02,
-0.0000000000e+00,
-1.5594989773e-02,
-0.0000000000e+00,
-1.3846153846e-02,
-0.0000000000e+00,
-1.2267936058e-02,
-0.0000000000e+00,
-1.0842111879e-02,
-0.0000000000e+00,
-9.5536620984e-03,
-0.0000000000e+00,
-8.3900579330e-03,
-0.0000000000e+00,
-7.3407216217e-03,
-0.0000000000e+00,
-6.3966145267e-03,
-0.0000000000e+00,
-5.5499192449e-03,
-0.0000000000e+00,
-4.7937919718e-03,
-0.0000000000e+00,
-4.1221680212e-03,
-0.0000000000e+00,
-3.5296080377e-03,
-0.0000000000e+00,
-3.0111756977e-03,
-0.0000000000e+00,
-2.5623400268e-03,
-0.0000000000e+00,
-2.1788971424e-03,
-0.0000000000e+00,
-1.8569074639e-03,
-0.0000000000e+00,
-1.5926453386e-03,
-0.0000000000e+00,
-1.3825587134e-03,
-0.0000000000e+00,
-1.2232369871e-03,
-0.0000000000e+00,
-1.1113855722e-03,
-0.0000000000e+00,
-1.0438059849e-03,
};
s16 *v34_rx_filters[12] = {
v34_rx_filter_2400_1600,
v34_rx_filter_2400_1800,
v34_rx_filter_2743_1646,
v34_rx_filter_2743_1829,
v34_rx_filter_2800_1680,
v34_rx_filter_2800_1867,
v34_rx_filter_3000_1800,
v34_rx_filter_3000_2000,
v34_rx_filter_3200_1829,
v34_rx_filter_3200_1920,
v34_rx_filter_3429_1959,
v34_rx_filter_3429_1959,
};
static void build_rx_filter(V34DSPState *s)
{
float a, f_low, f_high;
int i;
s->rx_filter = v34_rx_filters[s->S * 2 + s->use_high_carrier];
/* XXX: temporary hack to synchronize */
if (s->S == V34_S3429)
s->baud_phase = 1;
else
s->baud_phase = 2;
s->baud_num = (s->baud_num * 3);
s->carrier_incr = s->carrier_freq * (float)0x10000 / s->symbol_rate;
s->carrier_phase = 0;
s->rx_buf1_ptr = 0;
s->rx_filter_wsize = (s->baud_denom * RC_FILTER_SIZE) / s->baud_num;
printf("cincr=%d baudincr=%d\n", s->carrier_incr, s->baud_incr);
s->baud_phase = s->baud_phase << 16;
s->baud_num = s->baud_num << 16;
s->baud_denom = s->baud_denom << 16;
/* equalizer : init to identity */
s->eq_filter[EQ_SIZE/2][0] = 0x4000 << 16;
/* XXX: hilbert normalization ? */
for(i=0;i<EQ_SIZE;i++)
s->eq_filter[i][1] = (int)(hilbert[i] * 0.61475 * (0x4000 << 16));
/* adaptation shift : big at the beginning, should be small after. */
s->eq_shift = 0;
/* synchronization : Nyquist filters at the upper & lower frequencies */
a = 0.99;
f_low = 2 * M_PI * (s->carrier_freq - s->symbol_rate / 2.0) /
(3.0 * s->symbol_rate);
f_high = 2 * M_PI * (s->carrier_freq + s->symbol_rate / 2.0) /
(3.0 * s->symbol_rate);
printf("%f %f\n", f_low, f_high);
s->sync_low_coef[0] = (int)(2 * a * cos(f_low) * 0x4000);
s->sync_low_coef[1] = (int)( - a * a * 0x4000);
s->sync_high_coef[0] = (int)(2 * a * cos(f_high) * 0x4000);
s->sync_high_coef[1] = (int)(- a * a * 0x4000);
/* precomputed constants to compute the cross correlation */
s->sync_A = (int)( - a * a * sin(f_high - f_low) * 0x4000);
s->sync_B = (int)( a * sin(f_high) * 0x4000);
s->sync_C = (int)( - a * sin(f_low) * 0x4000);
}
int V34_init_low(V34DSPState *s, V34State *p, int transmit)
{
int S,d,e;
/* copy the params */
s->calling = p->calling;
s->S = p->S;
s->expanded_shape = p->expanded_shape;
s->R = p->R;
if (p->use_aux_channel)
s->R += 200;
s->conv_nb_states = p->conv_nb_states;
s->use_non_linear = p->use_non_linear;
s->use_high_carrier = p->use_high_carrier;
memcpy(s->h, p->h, sizeof(s->h));
/* superframe & data frame size */
S = s->S;
if (!s->use_high_carrier) {
d = S_tab[S][2];
e = S_tab[S][3];
} else {
d = S_tab[S][4];
e = S_tab[S][5];
}
s->symbol_rate = 2400.0 * (float)S_tab[S][0] / (float)S_tab[S][1];
s->carrier_freq = s->symbol_rate * (float)d / (float)e;
s->J = S_tab[S][6];
s->P = S_tab[S][7];
s->N = (s->R * 28) / (s->J * 100);
/* max length of a mapping frame (no need for table 8 as in the spec !) */
s->b = s->N / s->P;
if ((s->b * s->P) < s->N) s->b++;
s->r = s->N - (s->b - 1) * s->P;
/* aux channel */
if (p->use_aux_channel)
s->W = 15 - s->J; /* no need to test as in the spec ! */
else
s->W = 0; /* no aux channel */
/* mapping parameters */
s->q = 0;
if (s->b <= 12) {
s->K = 0;
} else {
s->K = s->b - 12;
while (s->K >= 32) {
s->K -= 8;
s->q++;
}
}
/* XXX: use integer arith ! */
if (!s->expanded_shape) {
s->M = (int) ceil(pow(2.0, s->K / 8.0));
} else {
s->M = (int) rint(1.25 * pow(2.0, s->K / 8.0));
}
s->L = 4 * s->M * (1 << s->q);
#ifdef DEBUG
printf("S_index=%d (S=%0.0f carrier=%0.0f)\n"
"R=%d J=%d P=%d N=%d b=%d r=%d W=%d\n",
s->S, s->symbol_rate, s->carrier_freq,
s->R, s->J, s->P, s->N, s->b, s->r, s->W);
printf("K=%d q=%d M=%d L=%d\n", s->K, s->q, s->M, s->L);
#endif
build_constellation(s);
build_rings(s);
s->baud_num = baud_tab[S][0];
s->baud_denom = baud_tab[S][1];
if (transmit) {
build_tx_filter(s);
} else {
build_rx_filter(s);
agc_init(s);
s->phase_4d = 0;
}
s->Z_1 = 0;
s->U0 = 0; /* trellis coder memory */
memset(s->x, 0, sizeof(s->x));
s->half_data_frame_count = 0;
s->sync_count = 0;
s->conv_reg = 0;
s->scrambler_reg = 0;
s->mapping_frame = 0; /* mapping frame counters */
s->acnt = 0;
s->rcnt = 0;
return 0;
}
/* shift by n & round toward zero */
static inline int shr_round0(int val, int n)
{
int offset;
offset = (1 << (n-1)) - 1;
if (val >= 0) {
val = (val + offset) >> n;
} else {
val = -val;
val = (val + offset) >> n;
val = -val;
}
return val;
}
/* clamp a between -v & v */
static inline int clamp(int a, int v)
{
if (a > v)
return v;
else if (a < -v)
return -v;
else
return a;
}
/* compute the next state in the trellis (Y[0] is ignored) */
static int trellis_next_state(int conv_nb_states, int conv_reg, int trans)
{
int i,Y0;
int Y[5];
Y[1] = (trans & 1);
Y[2] = ((trans >> 1) & 1);
Y[4] = ((trans >> 2) & 1);
Y[3] = ((trans >> 3) & 1);
Y0 = conv_reg & 1;
switch(conv_nb_states) {
case 16:
/* figure 10 */
conv_reg ^= (Y[1] << 1) | (Y[2] << 2) | ((Y[2] ^ Y0) << 3) | (Y0 << 4);
conv_reg >>= 1;
break;
case 32:
/* figure 11 */
conv_reg ^= (Y[2] << 1) | (Y[4] << 2) | (Y[1] << 3) | (Y[2] << 4) | (Y0 << 5);
conv_reg >>= 1;
break;
default:
case 64:
/* figure 12 */
{
int r[6], s[6], tmp1, tmp2;
for(i=0;i<6;i++) r[i] = (conv_reg >> i) & 1;
s[0] = r[1] ^ r[3] ^ Y[2];
s[1] = r[0];
s[2] = r[3];
tmp2 = Y[1] ^ r[4];
s[3] = r[3] ^ tmp2;
tmp1 = r[4] ^ r[5];
s[4] = r[2] ^ Y[3] ^ (r[3] & Y[2]) ^ tmp1;
s[5] = Y[4] ^ tmp1 ^ (r[3] & tmp2);
conv_reg = 0;
for(i=0;i<6;i++) conv_reg |= s[i] << i;
}
break;
}
return conv_reg;
}
/* (<28> 9.6.3) trellis encoder, return U0 */
static int trellis_encoder(V34DSPState *s, int c0, int yy[2][2])
{
int v0, ss[2][3], Y[5], i, trans;
/* convolutional coder */
/* (<28> 9.6.3.1) find Y vector. We traducted the table into binary expressions */
for(i=0;i<2;i++) {
int x,y,y0,x0,y1,x1;
/* XXX: is it right to suppose that we use figure 9 as a periodic mapping ? */
x = yy[i][0];
y = yy[i][1];
x = ((x + 3) >> 1) & 3;
y = ((y + 3) >> 1) & 3;
x0 = x & 1;
x1 = ((x & 2) >> 1);
y0 = y & 1;
y1 = ((y & 2) >> 1);
ss[i][2] = x1 ^ y1 ^ y0 ^ x0;
ss[i][1] = y0;
ss[i][0] = y0 ^ x0;
}
/* table 13 traducted into binary operations */
Y[4] = ss[0][2] ^ ss[1][2];
Y[3] = ss[0][1];
Y[2] = ss[0][0];
Y[1] = (ss[0][0] & ~ss[1][0] & 1) ^ ss[0][1] ^ ss[1][1];
/* compute the next trellis state */
trans = (Y[3] << 3) | (Y[4] << 2) | (Y[2] << 1) | Y[1];
s->conv_reg = trellis_next_state(s->conv_nb_states, s->conv_reg, trans);
Y[0] = s->conv_reg & 1;
/* super frame synchronisation pattern */
if (s->sync_count == 0) {
v0 = (SYNC_PATTERN >> (15 - s->half_data_frame_count)) & 1;
} else {
v0 = 0;
}
return Y[0] ^ c0 ^ v0;
}
static int get_bit(V34DSPState *s)
{
int b, poly;
b = s->get_bit(s->opaque);
if (b == -1) b = 1;
if (s->calling)
poly = V34_GPC;
else
poly = V34_GPA;
b = scramble_bit(s, b, poly);
return b;
}
/* auxilary channel bit */
static int aux_get_bit(V34DSPState *s)
{
return 0;
}
/* encode size bits into the corresponding symbols of the constellation */
/* return exactly 8 baseband complex samples */
static void encode_mapping_frame(V34DSPState *s)
{
int m[4][2], r0; /* rings */
u8 I[3][4];
int Q[4][2], Z[2], Y[2][2];
u8 *ptr;
int t,i,j,k,x,y,x1,y1,w,mp_size;
int u_re, u_im, p_re, p_im, c_re, c_im, C0, x_re, x_im, xp_re, xp_im;
u8 data[MAX_MAPPING_FRAME_SIZE];
/* compute mapping frame size */
s->rcnt += s->r;
if (s->rcnt < s->P) {
mp_size = s->b - 1;
} else {
s->rcnt -= s->P;
mp_size = s->b;
}
/* send an auxilary channel bit if needed */
s->acnt += s->W;
if (s->acnt < s->P) {
data[0] = get_bit(s);
} else {
s->acnt -= s->P;
data[0] = aux_get_bit(s);
}
for(i=1;i<mp_size;i++) data[i] = get_bit(s);
// print_bit_vector("sent", data, mp_size);
if (s->b <= 12) {
/* (<28> 9.3.2) simple case: no shell mapping */
memset(m, 0, sizeof(m));
for(i=0;i<s->b;i++) ((u8 *)I)[i] = data[i];
for(i=s->b;i<12;i++) ((u8 *)I)[i] = 0;
memset(Q, 0, sizeof(Q));
} else {
/* (<28> 9.3.1) */
/* leave one bit if low frame */
int n;
n = s->K;
if (mp_size < s->b) n--;
ptr = data;
r0 = 0;
for(i=0;i<n;i++) r0 |= *ptr++ << i;
index_to_rings(s, m, r0);
for(j=0;j<4;j++) {
I[0][j] = ptr[0];
I[1][j] = ptr[1];
I[2][j] = ptr[2];
ptr += 3;
t = 0;
for(i=0;i<s->q;i++) t |= *ptr++ << i;
Q[j][0] = t;
t = 0;
for(i=0;i<s->q;i++) t |= *ptr++ << i;
Q[j][1] = t;
}
}
if (s->b < 56)
w = 1;
else
w = 2;
for(j=0;j<4;j++) {
/* for each 4D symbol */
/* (<28> 9.5) differential coding */
Z[0] = (I[1][j] + 2 * I[2][j] + s->Z_1) & 3;
s->Z_1 = Z[0];
/* (<28> 9.6.1) mapping to 2D symbols */
Z[1] = (Z[0] + 2 * I[0][j] + s->U0) & 3;
C0 = 0; /* for trellis coding */
for(i=0;i<2;i++) {
t = Q[j][i] + (m[j][i] << s->q);
assert(t >= 0 && t < L_MAX/4);
x1 = s->constellation[t][0];
y1 = s->constellation[t][1];
/* rotation by Z[i] * 90 degress clockwise */
rotate_clockwise(x, y, x1, y1, Z[i]);
u_re = x;
u_im = y;
/* (<28> 9.6.2) precoder */
x = 0;
y = 0;
for(k=0;k<3;k++) {
x += s->x[k][0] * s->h[k][0] - s->x[k][1] * s->h[k][1];
y += s->x[k][1] * s->h[k][0] + s->x[k][0] * s->h[k][1];
}
/* round to 2^-7 */
p_re = shr_round0(x, 14);
p_im = shr_round0(y, 14);
/* compute c(n) */
c_re = shr_round0(p_re, 7 + w) << w;
c_im = shr_round0(p_im, 7 + w) << w;
C0 += c_re + c_im;
Y[i][0] = clamp(u_re + c_re, 255);
Y[i][1] = clamp(u_im + c_im, 255);
x_re = (Y[i][0] << 7) - p_re;
x_im = (Y[i][1] << 7) - p_im;
for(k=2;k>=1;k--) {
s->x[k][0] = s->x[k-1][0];
s->x[k][1] = s->x[k-1][1];
}
s->x[0][0] = x_re;
s->x[0][1] = x_im;
/* (<28> 9.7) non linear encoder */
if (!s->use_non_linear) {
xp_re = x_re;
xp_im = x_im;
} else {
int x2;
float dzeta,theta;
/* XXX: average power ? */
x2 = (x_re * x_re + x_im * x_im) >> 7;
dzeta = 0.3125 /* x2 / 128.0 */;
theta = (1 + dzeta / 6.0 + dzeta * dzeta / 120.0);
xp_re = rint(theta * x_re);
xp_im = rint(theta * x_im);
}
put_sym(s, xp_re, xp_im);
}
s->U0 = trellis_encoder(s, (C0 >> 1) & 1, Y);
/* 4D symbol count & data frame count for synchronisation */
if (++s->sync_count == 2*s->P) {
s->sync_count = 0;
if (++s->half_data_frame_count == 2*s->J) {
s->half_data_frame_count = 0;
}
}
}
if (++s->mapping_frame >= s->P) {
/* new data frame */
s->mapping_frame = 0;
s->rcnt = 0;
s->acnt = 0;
}
}
/* put a new baseband symbol in the tx queue */
static void put_sym(V34DSPState *s, int si, int sq)
{
s->tx_buf[s->tx_buf_ptr][0] = (si * s->tx_amp) >> 7;
s->tx_buf[s->tx_buf_ptr][1] = (sq * s->tx_amp) >> 7;
s->tx_buf_ptr = (s->tx_buf_ptr + 1) & (TX_BUF_SIZE - 1);
s->tx_buf_size++;
assert(s->tx_buf_size <= TX_BUF_SIZE);
}
/* write at most nb samples, return the number of samples
written. Stops if no more baseband symbols in tx queue */
static int V34_baseband_to_carrier(V34DSPState *s,
s16 *samples, unsigned int nb)
{
int si, sq, ph, i, j, k;
for(i=0;i<nb;i++) {
/* is there enough symbols in the queue ? */
if (s->tx_buf_size < s->tx_filter_wsize)
break;
/* apply the spectrum shaping filter */
ph = s->baud_phase;
si = sq = 0;
for(j=0;j<s->tx_filter_wsize;j++) {
k = (s->tx_outbuf_ptr - j - 1) &
(TX_BUF_SIZE - 1);
si += s->tx_buf[k][0] * s->tx_filter[ph];
sq += s->tx_buf[k][1] * s->tx_filter[ph];
ph += s->baud_denom;
}
si = si >> 14;
sq = sq >> 14;
if ( si != (short)si || sq != (short)sq) {
printf("error %d %d\n", si, sq);
}
// printf("M: phase=%04X %d %d\n", s->baud_phase, si, sq);
/* get next baseband symbols */
s->baud_phase += s->baud_num;
if (s->baud_phase >= s->baud_denom) {
s->baud_phase -= s->baud_denom;
s->tx_outbuf_ptr = (s->tx_outbuf_ptr + 1) & (TX_BUF_SIZE - 1);
s->tx_buf_size--;
}
/* center on the carrier */
samples[i] = (si * dsp_cos(s->carrier_phase) -
sq * dsp_cos((PHASE_BASE/4) - s->carrier_phase)) >> COS_BITS;
s->carrier_phase += s->carrier_incr;
}
return i;
}
#define S_POWER (1 + 1)
#define TRN4_POWER (1 + 1)
#define TRN16_POWER ((1 + 1 + 2*(9 + 1) + 9 + 9) / 4.0)
/* compute the normalized amplitude */
#define CALC_AMP(x) (int)( (128.0 * 128.0) / sqrt(x) )
/* send the V34 S sequence, duration: 128 T */
static void V34_send_S(V34DSPState *s)
{
int i;
/* transmit amplitude multiplier */
s->tx_amp = CALC_AMP(S_POWER);
for(i=0;i<64;i++) {
put_sym(s, 128, 128); /* 0 deg */
put_sym(s, -128, 128); /* -90 deg */
}
}
/* send the V34 S bar sequence, duration: 16 T */
static void V34_send_Sinv(V34DSPState *s)
{
int i;
s->tx_amp = CALC_AMP(S_POWER);
for(i=0;i<8;i++) {
put_sym(s, -128, -128); /* 180 deg */
put_sym(s, 128, -128); /* 90 deg */
}
}
/* send the PP sequence, duration: 288 T */
static void V34_send_PP(V34DSPState *s)
{
int k,p;
s->tx_amp = 128;
/* 6 periods */
for(p=0;p<6;p++) {
for(k=0;k<V34_PP_SIZE;k++) {
put_sym(s, tabPP[k].re, tabPP[k].im);
}
}
}
/* send the TRN sequence, (4 or 16 states), sent for 2048 T (XXX: this
time was choosen randomly */
static void V34_send_TRN(V34DSPState *s)
{
int i,x,y,x1,y1,z,poly,I1,I2,Q1,Q2,q;
if (s->calling)
poly = V34_GPC;
else
poly = V34_GPA;
if (s->is_16states)
s->tx_amp = CALC_AMP(TRN16_POWER);
else
s->tx_amp = CALC_AMP(TRN4_POWER);
for(i=0;i<1024;i++) {
I1 = scramble_bit(s, 1, poly);
I2 = scramble_bit(s, 1, poly);
if (s->is_16states) {
Q1 = scramble_bit(s, 1, poly);
Q2 = scramble_bit(s, 1, poly);
q = (Q2 << 1) | Q1;
} else {
q = 0;
}
x1 = s->constellation[q][0] << 7;
y1 = s->constellation[q][1] << 7;
z = (I2 << 1) | I1;
rotate_clockwise(x, y, x1, y1, z);
put_sym(s, x, y);
}
s->Z_1 = z; /* the last value z is used for the next sequence */
}
void put_bits(u8 **pp, int n, int bits)
{
u8 *p;
int i;
p = *pp;
for(i=n-1;i>=0;i--) {
*p++ = (bits >> i) & 1;
}
*pp = p;
}
/* from <20> 10.1.2.3.2 */
int calc_crc(u8 *buf, int size)
{
int crcinv, crc,i,b;
crc = 0xffff;
for(i=0;i<size;i++) {
b = (crc & 1) ^ buf[i];
crc = (crc >> 1) ^ ((b << 15) | (b << 10) | (b << 3));
}
/* invert the order of the crc bits (could be done while
computing, but who cares ? */
crcinv = 0;
for(i=0;i<16;i++) {
crcinv |= ((crc >> i) & 1) << (15-i);
}
return crcinv;
}
/* modulate an MP sequence */
static void V34_mod_MP(V34DSPState *s, u8 *buf, int size, int is_16states)
{
int x,y,x1,y1,z,poly,I1,I2,Q1,Q2,q;
u8 *p;
if (s->calling)
poly = V34_GPC;
else
poly = V34_GPA;
if (is_16states)
s->tx_amp = CALC_AMP(TRN16_POWER);
else
s->tx_amp = CALC_AMP(TRN4_POWER);
p = buf;
z = s->Z_1;
while ((p - buf) < size) {
I1 = scramble_bit(s, *p++, poly);
I2 = scramble_bit(s, *p++, poly);
if (is_16states) {
Q1 = scramble_bit(s, *p++, poly);
Q2 = scramble_bit(s, *p++, poly);
q = (Q2 << 1) | Q1;
} else {
q = 0;
}
x1 = s->constellation[q][0] << 7;
y1 = s->constellation[q][1] << 7;
z = (((I2 << 1) | I1) + z) & 3;
rotate_clockwise(x, y, x1, y1, z);
put_sym(s, x, y);
}
s->Z_1 = z;
}
/* send MP sequence. 'type' select its type (0 or 1). 'do_ack' selects
if it is an acknowledge sequence */
static void V34_send_MP(V34DSPState *s, int type, int do_ack)
{
u8 buf[188],*p;
int i,j,crc;
p = buf;
put_bits(&p, 17, 0x1ffff); /* frame sync */
put_bits(&p, 1, 0); /* start bit */
put_bits(&p, 1, type); /* type: 1 */
put_bits(&p, 1, 0); /* reserved */
put_bits(&p, 4, 12); /* call to answer max rate: 28800 */
put_bits(&p, 4, 12); /* answer to call max rate: 28800 */
put_bits(&p, 1, 0); /* no aux channel */
put_bits(&p, 2, 0); /* 0=16 state trellis */
put_bits(&p, 1, 0); /* non linear encoder disabled */
put_bits(&p, 1, 0); /* constellation shaping, 0=minimum, 1=expanded */
put_bits(&p, 1, do_ack); /* acknowledge bit */
put_bits(&p, 1, 0); /* start bit */
put_bits(&p, 15, 0x7ff8); /* all rates enabled up to 28800 */
put_bits(&p, 1, 1); /* asymetric data rate enable */
if (type == 1) {
for(i=0;i<3;i++) {
for(j=0;j<2;j++) {
put_bits(&p, 1, 0); /* start bit */
put_bits(&p, 16, s->h[i][j]); /* precoding coef */
}
}
}
put_bits(&p, 1, 0); /* start bit */
put_bits(&p, 16, 0); /* reserved by ITU */
put_bits(&p, 1, 0); /* start bit */
crc = calc_crc(buf + 17, p - (buf+17));
put_bits(&p, 16, crc);
put_bits(&p, 1, 0); /* fill bit */
if (type == 0) {
put_bits(&p, 2, 0); /* fill bit */
}
/* now we transmit the buffer */
V34_mod_MP(s, buf, p - buf, s->is_16states);
}
/* send E sequence */
static void V34_send_E(V34DSPState *s)
{
u8 buf[20];
memset(buf, 1, 20);
V34_mod_MP(s, buf, 20, s->is_16states);
}
/* J sequence */
#define J4POINTS 0x0991
#define J16POINTS 0x0D91
#define JEND 0xF991
static void V34_send_J(V34DSPState *s, int length)
{
int i,val;
u8 buf[16],*p;
if (s->is_16states)
val = J16POINTS;
else
val = J4POINTS;
p = buf;
put_bits(&p, 16, val);
for(i=0;i<length;i++) {
V34_mod_MP(s, buf, 16, 0); /* use 4 state modulation */
}
}
static void V34_send_JP(V34DSPState *s)
{
u8 buf[16],*p;
p = buf;
put_bits(&p, 16, JEND);
V34_mod_MP(s, buf, 16, 0);
}
static void V34_mod(V34DSPState *s, s16 *samples, unsigned int nb)
{
int n;
for(;;) {
/* modulate the symbols in the TX queue */
switch(s->state) {
case V34_STARTUP3_WAIT_J:
/* silence */
if (nb > 0) {
*samples++ = 0;
nb--;
}
break;
#if 0
case V34_START:
/* 600 bps DPSK modulation */
#endif
default:
/* V34 modulation */
while (s->tx_buf_size >= s->tx_filter_wsize && nb > 0) {
n = V34_baseband_to_carrier(s, samples, nb);
samples += n;
nb -= n;
}
break;
}
if (nb == 0) break;
/* protocol state machine */
switch(s->state) {
#if 0
/* phase 2 */
case V34_START:
V34_send_info0(s, 0);
break;
#endif
/* phase 3 */
case V34_STARTUP3_S1:
V34_send_S(s);
s->state = V34_STARTUP3_SINV1;
break;
case V34_STARTUP3_SINV1:
V34_send_Sinv(s);
s->state = V34_STARTUP3_S2;
break;
case V34_STARTUP3_S2:
V34_send_S(s);
s->state = V34_STARTUP3_SINV2;
break;
case V34_STARTUP3_SINV2:
V34_send_Sinv(s);
s->state = V34_STARTUP3_PP;
break;
case V34_STARTUP3_PP:
V34_send_PP(s);
s->state = V34_STARTUP3_TRN;
break;
case V34_STARTUP3_TRN:
V34_send_TRN(s);
// s->state = V34_STARTUP3_J;
break;
case V34_STARTUP3_J:
V34_send_J(s, 10); /* XXX: must wait for S on other end */
if (s->calling) {
s->state = V34_STARTUP3_JP;
} else {
/* the anwser modem wait for the S signal */
s->state = V34_STARTUP3_WAIT_J;
}
break;
case V34_STARTUP3_JP:
V34_send_JP(s);
//s->state = V34_STARTUP4_TRN;
s->state = V34_STARTUP3_S1;
break;
case V34_STARTUP3_WAIT_J:
if (s->J_received)
s->state = V34_STARTUP4_S;
break;
/* phase 4 */
case V34_STARTUP4_S:
V34_send_S(s);
s->state = V34_STARTUP4_WAIT_JP;
break;
case V34_STARTUP4_WAIT_JP:
if (s->JP_received)
s->state = V34_STARTUP4_SINV;
break;
case V34_STARTUP4_SINV:
V34_send_S(s);
s->state = V34_STARTUP4_TRN;
break;
case V34_STARTUP4_TRN:
V34_send_TRN(s);
s->state = V34_STARTUP4_MP;
break;
case V34_STARTUP4_MP:
V34_send_MP(s, 1, 0); /* type 1, no ack */
s->state = V34_STARTUP4_MPP;
break;
case V34_STARTUP4_MPP:
V34_send_MP(s, 1, 1); /* type 1, ack */
s->state = V34_STARTUP4_E;
break;
case V34_STARTUP4_E:
V34_send_E(s);
s->state = V34_DATA;
break;
case V34_DATA:
/* compute the next 8 baseband symbols */
encode_mapping_frame(s);
break;
}
}
}
static void V34_mod_init(V34DSPState *s, V34State *p)
{
V34_init_low(s, p, 1);
s->state = V34_STARTUP3_S1;
s->JP_received = 1;
// s->state = V34_DATA;
s->is_16states = 0; /* use 16 states */
}
/*****************************************************/
/* Here begins the real fun ! */
static inline int tcm_decision(int level, int sample)
{
int x, xs;
/* find the 4x4 subset */
x = (sample + (7 * 128) - (level << 8)) >> 10;
/* convert back to sample */
xs = (((x<<2) + level - 2) << 8) + 128;
return xs;
}
static int tcm_dist(int level, int sample)
{
int xs, e;
xs = tcm_decision(level,sample);
e = sample - xs;
#if 0
printf("level=%d sample=%d(%d) xs=%d e=%d e1=%d e2=%d\n",
level, sample, (sample >> 8) * 2 + 1, xs, e,
sample - (xs + 1024), sample-(xs - 1024));
#endif
return (e*e) >> 8;
}
/* Viterbi decoder for the V34 trellis coded modulation */
static void trellis_decoder(V34DSPState *s, s16 yout[2][2], s16 yy[2][2],
int *mse)
{
int i, j, k, n, nbbt, nb_trans, state, next_state, error, trellis_ptr;
int error_table[32],decision_table[32],emin,jmin,u0,x,y;
u8 *p,*q;
trellis_ptr = s->trellis_ptr;
/* compute the number of bits used in the transitions from each state */
switch(s->conv_nb_states) {
case 16:
nbbt = 2;
p = &trellis_trans_4[0][0];
break;
case 32:
nbbt = 3;
p = &trellis_trans_8[0][0];
break;
default:
nbbt = 4;
p = &trellis_trans_16[0][0];
break;
}
nb_trans = 1 << nbbt;
/* write a previous decoded symbol : extract a decoded bit from
the beginning of a path */
k = trellis_ptr;
k--;
if (k < 0) k = TRELLIS_LENGTH-1;
j = 0; /* arbitrary path selected : all the paths converge to same
decoded bits*/
for(i=0;i<(TRELLIS_LENGTH-1);i++) {
j = s->state_path[j][k];
k--;
if (k < 0) k = TRELLIS_LENGTH-1;
}
u0 = s->u0_memory[trellis_ptr];
q = p + (s->state_decision[j][k] * 4);
#if 1
yout[0][0] = tcm_decision(q[0], s->state_memory[trellis_ptr][0]);
yout[0][1] = tcm_decision(q[1], s->state_memory[trellis_ptr][1]);
yout[1][0] = tcm_decision(q[2], s->state_memory[trellis_ptr][2]);
yout[1][1] = tcm_decision(q[3], s->state_memory[trellis_ptr][3]);
/* undo the rotation */
if ((s->state_decision[j][k] >> 7)) {
x = yout[1][1];
y = - yout[1][0];
yout[1][0] = x;
yout[1][1] = y;
}
/* rotate only if u0 is set */
if (u0 ^ (s->state_decision[j][k] >> 7)) {
x = - yout[1][1];
y = yout[1][0];
yout[1][0] = x;
yout[1][1] = y;
}
#else
/* no trellis */
yout[0][0] = s->state_memory[trellis_ptr][0];
yout[0][1] = s->state_memory[trellis_ptr][1];
yout[1][0] = s->state_memory[trellis_ptr][2];
yout[1][1] = s->state_memory[trellis_ptr][3];
#endif
/* compute the mean square error (normalized to 2^7) */
*mse = (dsp_sqr(yout[0][0] - s->state_memory[trellis_ptr][0]) +
dsp_sqr(yout[0][1] - s->state_memory[trellis_ptr][1]) +
dsp_sqr(yout[1][0] - s->state_memory[trellis_ptr][2]) +
dsp_sqr(yout[1][1] - s->state_memory[trellis_ptr][3])) >> 7;
s->state_memory[trellis_ptr][0] = yy[0][0];
s->state_memory[trellis_ptr][1] = yy[0][1];
s->state_memory[trellis_ptr][2] = yy[1][0];
s->state_memory[trellis_ptr][3] = yy[1][1];
/* compute the error table */
/* XXX: may be optimized by using the algebraic properties of the mapping */
n = 128 >> nbbt;
jmin = 0; /* no warning */
for(i=0;i<(nb_trans*2);i++) {
emin = 0x7fffffff;
for(j=0;j<n;j++) {
int e;
e = tcm_dist(p[0], yy[0][0]) +
tcm_dist(p[1], yy[0][1]) +
tcm_dist(p[2], yy[1][0]) +
tcm_dist(p[3], yy[1][1]);
if (e < emin) {
emin = e;
jmin = j;
}
p+=4;
}
error_table[i] = emin;
decision_table[i] = i * n + jmin;
}
/* we compute the bit u0 (needed for synchronization & c0 estimation) */
jmin = 0;
emin = 0x7fffffff;
for(i=0;i<nb_trans*2;i++) {
if (error_table[i] < emin) {
jmin = i;
emin = error_table[i];
}
}
s->u0_memory[trellis_ptr] = (jmin >= nb_trans);
/* init the error table to +infinity */
for(state=0;state<s->conv_nb_states;state++) {
s->state_error1[state] = 0x7fffffff;
}
for(state=0;state<s->conv_nb_states;state++) {
/* select the value of y0 depending on the current state */
/* for each state, we update the next state entry by selecting
the shortest path */
/* XXX: should handle error overflow */
if (state & 1)
n = nb_trans;
else
n = 0;
for(j=0;j<nb_trans;j++) {
next_state = trellis_next_state(s->conv_nb_states, state, j);
error = s->state_error[state] + error_table[j + n];
if (error < s->state_error1[next_state]) {
s->state_error1[next_state] = error;
s->state_decision[next_state][trellis_ptr] = decision_table[j + n];
s->state_path[next_state][trellis_ptr] = state;
}
}
}
/* XXX: this copy is not needed. Permute the two tables */
memcpy(s->state_error, s->state_error1, sizeof(s->state_error));
trellis_ptr = (trellis_ptr + 1) % TRELLIS_LENGTH;
s->trellis_ptr = trellis_ptr;
}
static void put_bit(V34DSPState *s, int b)
{
int poly;
if (!s->calling)
poly = V34_GPC;
else
poly = V34_GPA;
b = unscramble_bit(s, b, poly);
// printf("recv: %d\n", b);
s->put_bit(s->opaque, b);
}
/* auxilary channel bit */
static void aux_put_bit(V34DSPState *s, int b)
{
/* not used now */
}
static void decode_mapping_frame(V34DSPState *s, s16 rx_mapping_frame[8][2])
{
int m[4][2], r0; /* rings */
u8 I[3][4];
int Q[4][2], Z[2];
u8 *ptr;
int t,i,j,x,y,n,mp_size,xout_ptr;
u8 data[MAX_MAPPING_FRAME_SIZE];
xout_ptr = 0;
for(j=0;j<4;j++) {
/* for each 4D symbol */
for(i=0;i<2;i++) {
x = rx_mapping_frame[xout_ptr][0];
y = rx_mapping_frame[xout_ptr][1];
xout_ptr++;
/* decision */
x = (x >> 8) * 2 + 1;
x = clamp(x, C_RADIUS);
y = (y >> 8) * 2 + 1;
y = clamp(y, C_RADIUS);
t = s->constellation_to_code[(x+C_RADIUS) >> 1][(y+C_RADIUS) >> 1];
/* mapping to the symbol */
Z[i] = t >> 14;
t = t & 0xff;
Q[j][i] = t & ((1 << s->q)-1);
m[j][i] = t >> s->q;
}
t = (Z[0] - s->Z_1) & 3;
s->Z_1 = Z[0];
I[1][j] = t & 1;
I[2][j] = t >> 1;
t = (Z[1] - Z[0]) & 3;
I[0][j] = t >> 1;
}
/* compute mapping frame size */
s->rcnt += s->r;
if (s->rcnt < s->P) {
mp_size = s->b - 1;
} else {
s->rcnt -= s->P;
mp_size = s->b;
}
/* now everything is "decoded", we can write the data */
ptr = data;
if (s->b <= 12) {
for(i=0;i<s->b;i++) *ptr++ = ((u8 *)I)[i];
} else {
r0 = rings_to_index(s, m);
n = s->K;
if (mp_size < s->b) n--;
for(i=0;i<n;i++) *ptr++ = (r0 >> i) & 1;
for(j=0;j<4;j++) {
ptr[0] = I[0][j];
ptr[1] = I[1][j];
ptr[2] = I[2][j];
ptr += 3;
t=Q[j][0];
for(i=0;i<s->q;i++) *ptr++ = (t >> i) & 1;
t=Q[j][1];
for(i=0;i<s->q;i++) *ptr++ = (t >> i) & 1;
}
}
/* send an auxilary channel bit if needed */
s->acnt += s->W;
if (s->acnt < s->P) {
put_bit(s, data[0]);
} else {
s->acnt -= s->P;
aux_put_bit(s, data[0]);
}
/* send all the decoded bits */
for(i=1;i<mp_size;i++) put_bit(s, data[i]);
// print_bit_vector("recv", data, mp_size);
if (++s->mapping_frame >= s->P) {
/* new data frame */
s->mapping_frame = 0;
s->rcnt = 0;
s->acnt = 0;
}
}
static void baseband_decode(V34DSPState *s, int si, int sq)
{
s16 y[2][2];
static int delay = 0;
int mse,v0;
lm_dump_qam(si / (10.0 * 128.0), sq / (10.0 * 128.0));
s->yy[s->phase_4d][0] = si;
s->yy[s->phase_4d][1] = sq;
if (++s->phase_4d == 2) {
trellis_decoder(s, y, s->yy , &mse);
s->phase_mse += mse;
s->phase_mse_cnt++;
if (s->phase_mse_cnt >= 8) {
s->phase_mse_cnt = 0;
s->phase_mse = 0;
}
/* synchronization bit */
if (s->sync_count == 0) {
v0 = (SYNC_PATTERN >> (15 - s->half_data_frame_count)) & 1;
} else {
v0 = 0;
}
/* synchronization bit */
if (++s->sync_count == 2*s->P) {
s->sync_count = 0;
if (++s->half_data_frame_count == 2*s->J) {
s->half_data_frame_count = 0;
}
}
memcpy(&s->rx_mapping_frame[s->rx_mapping_frame_count][0],
&y[0][0], 4 * sizeof(s16));
delay++;
if (delay > TRELLIS_LENGTH) {
s->rx_mapping_frame_count += 2;
if (s->rx_mapping_frame_count == 8) {
/* a complete mapping frame was read */
decode_mapping_frame(s, s->rx_mapping_frame);
s->rx_mapping_frame_count = 0;
}
}
s->phase_4d = 0;
}
}
/* AGC */
#define AGC_COEF 0.99
#define AGC_BITS 24
static void agc_init(V34DSPState *s)
{
s->agc_coef = (int) (AGC_COEF * (1 << AGC_BITS));
s->agc_mem = 0;
}
static void agc_estimate(V34DSPState *s, int sample)
{
float power;
s->agc_mem = s->agc_mem * AGC_COEF + (sample * sample);
power = (float) s->agc_mem * (1.0 - AGC_COEF);
/* XXX: the constant here depends on the modulation parameters */
// s->agc_gain = (16384.0/2 * 1495) / sqrt(power);
s->agc_gain = 16384.0 * 0.80;
// lm_dump_agc(power / 16384.0);
lm_dump_agc(sqrt(power));
}
#if 0
static void test_nyq(float *a_ptr, float *b_ptr, float f, float val)
{
float a,b,c,d;
a = *a_ptr;
b = *b_ptr;
c = 0.99 * cos(f);
d = 0.99 * sin(f);
*a_ptr = a * c - b * d + val;
*b_ptr = a * d + b * c;
}
static float al, bl, ah, bh;
static float f_low, f_high;
static float low_mem[2], low_coef[2];
#endif
#define SYNC_THR 32
/* Fast symbol timing recovery */
static void v34_symbol_sync(V34DSPState *s, int spl)
{
int a, b, c, v;
#if 0
int tmp0, tmp1;
static s16 dc_filter[2];
static s16 ac_filter[3];
#endif
#if 0
{
static int k = 0;
float v;
f_low = 2 * M_PI * (s->carrier_freq - s->symbol_rate / 2.0) / (3.0 * s->symbol_rate);
f_high = 2 * M_PI * (s->carrier_freq + s->symbol_rate / 2.0) / (3.0 * s->symbol_rate);
test_nyq(&al, &bl, f_low, spl);
test_nyq(&ah, &bh, f_high, spl);
}
#endif
/* sync_low_mem has an amplitude of about spl / ( 1 - a), so we
store it with a scaling of 2^6 to reduce its amplitude */
v = ((spl << 8) + s->sync_low_mem[0] * s->sync_low_coef[0] +
s->sync_low_mem[1] * s->sync_low_coef[1]) >> 14;
s->sync_low_mem[1] = s->sync_low_mem[0];
s->sync_low_mem[0] = v;
v = ((spl << 8) + s->sync_high_mem[0] * s->sync_high_coef[0] +
s->sync_high_mem[1] * s->sync_high_coef[1]) >> 14;
s->sync_high_mem[1] = s->sync_high_mem[0];
s->sync_high_mem[0] = v;
if (s->baud3_phase == 0) {
/* high & low nyquist filters for symbol timing recovery */
/* XXX: the shift should adapt to the power */
a = (s->sync_low_mem[1] * s->sync_high_mem[1]) >> 14;
b = (s->sync_high_mem[1] * s->sync_low_mem[0]) >> 14;
c = (s->sync_low_mem[1] * s->sync_high_mem[0]) >> 14;
v = (a * s->sync_A + b * s->sync_B + c * s->sync_C) >> 14;
printf("v=%d\n", v);
s->baud_phase -= v << 3;
#if 0
/* DC filter h(z) = z - z^-2 */
tmp0 = v - dc_filter[1];
dc_filter[1] = dc_filter[0];
dc_filter[0] = v;
/* AC filter h(z) = z + z^-3 */
tmp1 = tmp0 + ac_filter[2];
ac_filter[2] = ac_filter[1];
ac_filter[1] = ac_filter[0];
ac_filter[0] = tmp0;
if (tmp1 < -SYNC_THR)
tmp1 = -SYNC_THR;
else if (tmp1 > SYNC_THR)
tmp1 = SYNC_THR;
else
tmp1 =0;
#endif
#if 1
// lm_dump_sample(CHANNEL_SAMPLE, v);
// printf("corr=%0.0f\n",
// corr * 32.0 / 100.0);
#else
printf("al=%0.1f ah=%0.1f al1=%0.1f ah1=%0.1f\n",
ah / 100.0, bh / 100.0,
(s->sync_high_mem[0] - s->sync_high_mem[1] * 0.99 * cos(f_high)) * 32.0 / 100.0,
(s->sync_high_mem[1] * 0.99 * sin(f_high)) * 32.0 / 100.0);
#endif
}
}
/* equalize & adapt the equalizer */
static int v34_equalize(V34DSPState *s,
int *ri_ptr, int *rq_ptr, int spl)
{
int p,q,i;
int ri, rq, fi, fq, q_ri, q_rq, ei, eq, ei1, eq1, si, sq;
int cosw, sinw, dphi, norm;
/* add the sample in the equalizer ring buffer */
p = s->eq_buf_ptr;
s->eq_buf[p] = spl;
if (++p == EQ_SIZE)
p = 0;
s->eq_buf_ptr = p;
if (s->baud3_phase != 0)
return 0;
/* apply the equalizer filter to the data */
ri = rq = 0;
q = p;
for(i=0;i<EQ_SIZE;i++) {
fi = s->eq_filter[i][0] >> 16;
fq = s->eq_filter[i][1] >> 16;
ri += fi * s->eq_buf[q];
rq += fq * s->eq_buf[q];
q++;
if (q == EQ_SIZE)
q = 0;
}
si = ri >> 14;
sq = rq >> 14;
/* rotate by the carrier phase */
/* translate back to baseband */
cosw = dsp_cos(s->carrier_phase);
sinw = - dsp_cos((PHASE_BASE/4) - s->carrier_phase);
ri = ( si * cosw - sq * sinw ) >> COS_BITS;
rq = ( si * sinw + sq * cosw ) >> COS_BITS;
*ri_ptr = ri;
*rq_ptr = rq;
/* compute the error */
/* quantification */
q_ri = ((ri >> 8) * 2 + 1) << 7;
q_rq = ((rq >> 8) * 2 + 1) << 7;
/* error computation */
ei1 = - (ri - q_ri);
eq1 = - (rq - q_rq);
/**** phase tracking */
/* normalized derivative of the phase shift */
/* XXX: avoid sqrt : slow !!! */
norm = (int) sqrt(ri * ri + rq * rq );
if (norm > 0) {
dphi = (ri * eq1 - rq * ei1) / norm;
} else {
dphi = 0;
}
s->carrier_phase += s->carrier_incr - dphi;
/* remodulate (because the equalizer is done before converting to
baseband) */
ei = ( ei1 * cosw + eq1 * sinw ) >> COS_BITS;
eq = ( - ei1 * sinw + eq1 * cosw ) >> COS_BITS;
#if 1
/* update the coefficients with the error */
q = p;
for(i=0;i<EQ_SIZE;i++) {
int di, dq;
di = ei * s->eq_buf[q];
dq = eq * s->eq_buf[q];
s->eq_filter[i][0] += (di >> s->eq_shift) * 16;
s->eq_filter[i][1] += (dq >> s->eq_shift) * 16;
q++;
if (q == EQ_SIZE)
q = 0;
}
#endif
lm_dump_equalizer(s->eq_filter, 1 << 30, EQ_SIZE);
return 1;
}
static void V34_demod(V34DSPState *s,
const s16 *samples, unsigned int nb)
{
int si, sq, i, j, k , ph, spl;
int v, frac, ph1;
for(i=0;i<nb;i++) {
/* Automatic Gain Control */
spl = samples[i];
agc_estimate(s, spl);
spl = (spl * s->agc_gain) >> 14;
/* insert the new sample in the ring buffer */
s->rx_buf1[s->rx_buf1_ptr] = spl;
s->rx_buf1_ptr = (s->rx_buf1_ptr + 1) & (RX_BUF1_SIZE-1);
/* sample rate convertion, timing correction & matched filter
(root raised cosine) */
s->baud_phase += s->baud_num;
while (s->baud_phase >= s->baud_denom) {
s->baud_phase -= s->baud_denom;
ph = s->baud_phase;
si = 0;
for(j=0;j<s->rx_filter_wsize;j++) {
k = (s->rx_buf1_ptr - s->rx_filter_wsize + j) & (RX_BUF1_SIZE-1);
/* XXX: verify that there is no overflow */
/* interpolation of the filter coefficient */
ph1 = ph >> 16;
frac = ph & 0xffff;
v = ((0x10000 - frac) * s->rx_filter[ph1] +
frac * s->rx_filter[ph1+1]) >> 16;
si += v * s->rx_buf1[k];
ph += s->baud_num;
}
si = (si >> 14);
lm_dump_sample(CHANNEL_SAMPLESYNC, si / 32768.0);
/* we have here EQ_FRAC = 3 symbols per baud */
switch(s->state) {
case V34_STARTUP3_WAIT_S1:
/* wait for the S signal */
printf("waiting S1 %d\n", si);
/* XXX: find a better test ! */
if (abs(si) > 13000) {
s->state = V34_STARTUP3_S1;
s->sym_count = 0;
}
break;
case V34_STARTUP3_S1:
/* S signals are mainly used to recover the symbol clock */
v34_symbol_sync(s, si);
if (++s->sym_count >= 128 * EQ_FRAC) {
s->state = V34_STARTUP3_SINV1;
s->sym_count = 0;
}
break;
case V34_STARTUP3_SINV1:
v34_symbol_sync(s, si);
if (++s->sym_count >= 16 * EQ_FRAC) {
s->state = V34_STARTUP3_S2;
s->sym_count = 0;
}
break;
case V34_STARTUP3_S2:
v34_symbol_sync(s, si);
if (++s->sym_count >= 128 * EQ_FRAC) {
s->state = V34_STARTUP3_SINV2;
s->sym_count = 0;
}
break;
case V34_STARTUP3_SINV2:
v34_symbol_sync(s, si);
if (++s->sym_count >= 100 * EQ_FRAC) {
s->state = V34_STARTUP3_PP;
s->sym_count = 0;
}
break;
case V34_STARTUP3_PP:
#if 1
/* PP is used to fast train the equalizer */
/* store the 144 samples at the middle of the PP
frame. We do this because we suppose in the fast
equalizer that the sequence is periodic */
if (s->sym_count >= (120) * EQ_FRAC &&
s->sym_count < (168) * EQ_FRAC) {
s->eq_buf[s->sym_count - (120) * EQ_FRAC] = si;
}
if (s->sym_count == (168) * EQ_FRAC) {
/* XXX: this call takes a long time. Is it a
problem ? */
V34_fast_equalize(s, s->eq_buf);
/* reset eq_buf to avoid potential problems when the
adaptive is started */
memset(s->eq_buf, 0, sizeof(s->eq_buf));
}
if (++s->sym_count == 288 * EQ_FRAC) {
s->state = V34_STARTUP3_TRN;
}
#else
memmove(s->eq_buf, &s->eq_buf[1], 2 * EQ_SIZE);
s->eq_buf[EQ_SIZE - 1] = si;
if ((++s->sym_count % EQ_SIZE) == 0) {
V34_fast_equalize(s, s->eq_buf);
lm_dump_equalizer(s->eq_filter, 1 << 30, EQ_SIZE);
}
#endif
break;
case V34_STARTUP3_TRN:
si = (float)si * 128.0 / CALC_AMP(TRN4_POWER);
if (v34_equalize(s, &si, &sq, si)) {
static int ptr = 0;
if (++ptr > (28 * 2)) {
baseband_decode(s, si, sq);
}
}
break;
}
if (++s->baud3_phase == EQ_FRAC)
s->baud3_phase = 0;
}
}
}
static void V34_demod_init(V34DSPState *s, V34State *p)
{
memset(s, 0, sizeof(V34DSPState));
V34_init_low(s, p, 0);
s->state = V34_STARTUP3_WAIT_S1;
}
/* init the V34 constants. Should be launched once */
void V34_static_init(void)
{
V34eq_init();
}
void V34_init(struct V34State *s, int calling)
{
}
int V34_process(struct V34State *s, s16 *output, s16 *input, int nb_samples)
{
return 0;
}
/* V34 test: half duplex with fixed parameters */
#define NB_SAMPLES 40 /* 5 ms */
static int test_get_bit(void *opaque)
{
return 1;
}
static int nb_bits, errors;
static void test_put_bit(void *opaque, int bit)
{
nb_bits++;
if (bit != 1) {
errors++;
}
}
void V34_test(void)
{
V34State s;
V34DSPState v34_rx, v34_tx;
int err;
struct LineModelState *line_state;
s16 buf[NB_SAMPLES];
s16 buf1[NB_SAMPLES];
s16 buf2[NB_SAMPLES];
s16 buf3[NB_SAMPLES];
FILE *f1;
err = lm_display_init();
if (err < 0) {
fprintf(stderr, "Could not init X display\n");
exit(1);
}
line_state = line_model_init();
/* fill the test V34 parameters */
#if 0
s.S = V34_S3200;
s.R = 28800;
#else
s.S = V34_S2400;
s.R = 19200;
#endif
s.expanded_shape = 0;
s.conv_nb_states = 16;
s.use_non_linear = 0;
s.use_high_carrier = 1;
s.use_aux_channel = 0;
memset(s.h, 0, sizeof(s.h));
f1 = fopen("cal.sw", "wb");
if (f1 == NULL) {
perror("cal.sw");
exit(1);
}
s.calling = 1;
V34_mod_init(&v34_tx, &s);
v34_tx.opaque = NULL;
v34_tx.get_bit = test_get_bit;
s.calling = 0;
V34_demod_init(&v34_rx, &s);
v34_rx.opaque = NULL;
v34_rx.put_bit = test_put_bit;
nb_bits = 0;
errors = 0;
for(;;) {
if (lm_display_poll_event())
break;
V34_mod(&v34_tx, buf, NB_SAMPLES);
memset(buf3, 0, sizeof(buf3));
line_model(line_state, buf1, buf, buf2, buf3, NB_SAMPLES);
fwrite(buf, 1, NB_SAMPLES * 2, f1);
V34_demod(&v34_rx, buf, NB_SAMPLES);
}
fclose(f1);
printf("errors=%d nb_bits=%d Pe=%f\n",
errors, nb_bits, (float) errors / (float)nb_bits);
}
View Git Blame Copy Permalink