1695 lines
45 KiB
C
1695 lines
45 KiB
C
/* implementation of UK0 Bus
|
|
*
|
|
* (C) 2022 by Andreas Eversberg <jolly@eversberg.eu>
|
|
* All Rights Reserved
|
|
*
|
|
* This program is free software: you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation, either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful,
|
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
* GNU General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License
|
|
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
#include <unistd.h>
|
|
#include <errno.h>
|
|
#include <stdint.h>
|
|
#include <stdlib.h>
|
|
#include <math.h>
|
|
#include <libdebug/debug.h>
|
|
#include <libsample/sample.h>
|
|
#include "uk0.h"
|
|
|
|
#define CHAN uk0->name
|
|
|
|
#define SYMBOL_RATE 120000.0
|
|
#define FRAME_RATE 1000
|
|
#define TX_LEVEL 0.85 /* level must be below 1, so that filters inside sound card will not overdrive signal */
|
|
#define RX_SINC_WINDOW 20
|
|
#define RX_OVERSAMPLING 10
|
|
//#define RX_CLOCK_FILTER (SYMBOL_RATE * 0.0001) /* 100 ppm */
|
|
#define RX_CLOCK_FILTER 1000.0
|
|
|
|
#define MIN_LEVEL 0.2
|
|
|
|
#define SYNC_MASK 0x3fffff
|
|
#define SYNC_LT_POSITIVE 0x269a95
|
|
#define SYNC_LT_NEGATIVE 0x19656a
|
|
#define SYNC_NT_POSITIVE 0x15a9a6
|
|
#define SYNC_NT_NEGATIVE 0x2a5659
|
|
|
|
#define WAKEUP 0x5555aaaa5555aaaa
|
|
|
|
#define LOS_CRITERION (120 * 2) /* between 1 and 2 ms */
|
|
|
|
#define SYNC_RECOVER 2 /* if <given> subsequent syncs are not received when expected, hut for a new one */
|
|
|
|
/* uncomment this to debug 4B3T */
|
|
//#define DEBUG_TABLE
|
|
|
|
/* uncomment to debug rx sound */
|
|
//#define DEBUG_RECORD_RX
|
|
|
|
/* uncomment to use upsampled signal when debugging rx sound */
|
|
//#define DEBUG_RECORD_UPSAMPLE
|
|
|
|
/* uncomment this to debug frame decoding and encoding */
|
|
//#define DEBUG_FRAME
|
|
|
|
/* uncomment this to debug scrambler and descrambler */
|
|
//#define DEBUG_SCRAMBLER
|
|
|
|
/* uncomment this to loop HDLC back to upper layer */
|
|
//#define HDLC_LOOPBACK
|
|
|
|
/*
|
|
* tables for transcoding
|
|
*/
|
|
|
|
struct mms43_def {
|
|
const char *bits;
|
|
struct {
|
|
const char *symbols;
|
|
int next;
|
|
} column[4];
|
|
} mms43_def[16] = {
|
|
{ "0001", { { "0-+", 1 }, { "0-+", 2 }, { "0-+", 3 }, { "0-+", 4 } } },
|
|
{ "0111", { { "-0+", 1 }, { "-0+", 2 }, { "-0+", 3 }, { "-0+", 4 } } },
|
|
{ "0100", { { "-+0", 1 }, { "-+0", 2 }, { "-+0", 3 }, { "-+0", 4 } } },
|
|
{ "0010", { { "+-0", 1 }, { "+-0", 2 }, { "+-0", 3 }, { "+-0", 4 } } },
|
|
{ "1011", { { "+0-", 1 }, { "+0-", 2 }, { "+0-", 3 }, { "+0-", 4 } } },
|
|
{ "1110", { { "0+-", 1 }, { "0+-", 2 }, { "0+-", 3 }, { "0+-", 4 } } },
|
|
{ "1001", { { "+-+", 2 }, { "+-+", 3 }, { "+-+", 4 }, { "---", 1 } } },
|
|
{ "0011", { { "00+", 2 }, { "00+", 3 }, { "00+", 4 }, { "--0", 2 } } },
|
|
{ "1101", { { "0+0", 2 }, { "0+0", 3 }, { "0+0", 4 }, { "-0-", 2 } } },
|
|
{ "1000", { { "+00", 2 }, { "+00", 3 }, { "+00", 4 }, { "0--", 2 } } },
|
|
{ "0110", { { "-++", 2 }, { "-++", 3 }, { "--+", 2 }, { "--+", 3 } } },
|
|
{ "1010", { { "++-", 2 }, { "++-", 3 }, { "+--", 2 }, { "+--", 3 } } },
|
|
{ "1111", { { "++0", 3 }, { "00-", 1 }, { "00-", 2 }, { "00-", 3 } } },
|
|
{ "0000", { { "+0+", 3 }, { "0-0", 1 }, { "0-0", 2 }, { "0-0", 3 } } },
|
|
{ "0101", { { "0++", 3 }, { "-00", 1 }, { "-00", 2 }, { "-00", 3 } } },
|
|
{ "1100", { { "+++", 4 }, { "-+-", 1 }, { "-+-", 2 }, { "-+-", 3 } } },
|
|
};
|
|
|
|
/* encoding table: the 6 lower bits define three symbols (0 = 0, + = 1, - = 2), the 2 upper bits define next column */
|
|
uint8_t mms43_encode[16][4];
|
|
|
|
/* decoding table: the 3 symbols result in 4 bits */
|
|
uint8_t mms43_decode[64];
|
|
|
|
static void generate_mms43_code(void)
|
|
{
|
|
uint8_t bits, symbols;
|
|
uint16_t used_bits = 0;
|
|
int i, j, k;
|
|
|
|
/* encoding table */
|
|
for (i = 0; i < 16; i++) {
|
|
bits = mms43_def[i].bits[0] - '0';
|
|
bits |= (mms43_def[i].bits[1] - '0') << 1;
|
|
bits |= (mms43_def[i].bits[2] - '0') << 2;
|
|
bits |= (mms43_def[i].bits[3] - '0') << 3;
|
|
used_bits |= 1 << bits;
|
|
for (j = 0; j < 4; j++) {
|
|
symbols = (mms43_def[i].column[j].next - 1) << 6;
|
|
for (k = 0; k < 3; k++) {
|
|
switch (mms43_def[i].column[j].symbols[k]) {
|
|
case '0':
|
|
break;
|
|
case '+':
|
|
symbols |= 1 << (k * 2);
|
|
break;
|
|
case '-':
|
|
symbols |= 2 << (k * 2);
|
|
break;
|
|
default:
|
|
PDEBUG(DUK0, DEBUG_ERROR, "Illegal symbol, please fix!\n");
|
|
abort();
|
|
}
|
|
}
|
|
mms43_encode[bits][j] = symbols;
|
|
}
|
|
}
|
|
if (used_bits != 0xffff) {
|
|
PDEBUG(DUK0, DEBUG_ERROR, "Not all 16 different bit combinations are defined, please fix!\n");
|
|
abort();
|
|
}
|
|
|
|
/* decoding table */
|
|
#ifdef DEBUG_TABLE
|
|
printf("encoding:\n");
|
|
#endif
|
|
memset(mms43_decode, 0xff, 64);
|
|
for (i = 0; i < 16; i++) {
|
|
#ifdef DEBUG_TABLE
|
|
printf("%d%d%d%d: ", i & 1, (i >> 1) & 1, (i >> 2) & 1, (i >> 3) & 1);
|
|
#endif
|
|
for (j = 0; j < 4; j++) {
|
|
mms43_decode[mms43_encode[i][j] & 0x3f] = i;
|
|
for (k = 0; k < 3; k++) {
|
|
switch((mms43_encode[i][j] >> (k * 2)) & 3) {
|
|
case 1:
|
|
#ifdef DEBUG_TABLE
|
|
printf("+");
|
|
#endif
|
|
break;
|
|
case 2:
|
|
#ifdef DEBUG_TABLE
|
|
printf("-");
|
|
#endif
|
|
break;
|
|
case 0:
|
|
#ifdef DEBUG_TABLE
|
|
printf("0");
|
|
#endif
|
|
break;
|
|
default:
|
|
#ifdef DEBUG_TABLE
|
|
printf("?");
|
|
#endif
|
|
;
|
|
}
|
|
}
|
|
#ifdef DEBUG_TABLE
|
|
printf(" %d ", ((mms43_encode[i][j] >> (k * 2)) & 3) + 1);
|
|
#endif
|
|
}
|
|
#ifdef DEBUG_TABLE
|
|
printf("\n");
|
|
#endif
|
|
}
|
|
mms43_decode[0] = 0; /* special case T(000)->B(0000) */
|
|
|
|
#ifdef DEBUG_TABLE
|
|
printf("decoding:\n");
|
|
#endif
|
|
for (i = 0; i < 16; i++) {
|
|
#ifdef DEBUG_TABLE
|
|
printf("%d%d%d%d: ", i & 1, (i >> 1) & 1, (i >> 2) & 1, (i >> 3) & 1);
|
|
#endif
|
|
for (j = 0; j < 64; j++) {
|
|
if (mms43_decode[j] == i) {
|
|
for (k = 0; k < 3; k++) {
|
|
switch((j >> (k * 2)) & 3) {
|
|
case 1:
|
|
#ifdef DEBUG_TABLE
|
|
printf("+");
|
|
#endif
|
|
break;
|
|
case 2:
|
|
#ifdef DEBUG_TABLE
|
|
printf("-");
|
|
#endif
|
|
break;
|
|
case 0:
|
|
#ifdef DEBUG_TABLE
|
|
printf("0");
|
|
#endif
|
|
break;
|
|
default:
|
|
#ifdef DEBUG_TABLE
|
|
printf("?");
|
|
#endif
|
|
;
|
|
}
|
|
}
|
|
#ifdef DEBUG_TABLE
|
|
printf(" ");
|
|
#endif
|
|
}
|
|
}
|
|
#ifdef DEBUG_TABLE
|
|
printf("\n");
|
|
#endif
|
|
}
|
|
#ifdef DEBUG_TABLE
|
|
exit(0);
|
|
#endif
|
|
}
|
|
|
|
/*
|
|
* instance creation and destruction
|
|
*/
|
|
|
|
/* init globals */
|
|
void uk0_init(void)
|
|
{
|
|
generate_mms43_code();
|
|
}
|
|
|
|
/* open UK0 Interface */
|
|
uk0_t *uk0_create(const char *name, int samplerate, int buffer_size)
|
|
{
|
|
uk0_t *uk0;
|
|
uk0_tx_t *tx;
|
|
uk0_rx_t *rx;
|
|
double x, l;
|
|
int i;
|
|
|
|
uk0 = calloc(1, sizeof(*uk0));
|
|
if (!uk0)
|
|
return NULL;
|
|
tx = &uk0->tx;
|
|
rx = &uk0->rx;
|
|
|
|
uk0->name = name;
|
|
|
|
/* delay that is greater than sound card loop (in milliseconds), also there must be some frames to wait until we receive framing */
|
|
uk0->ti1 = buffer_size * 1000 / samplerate + 32; /* sound card delay + 32 frames */
|
|
|
|
/* one ms ti2 */
|
|
uk0->ti2 = 1;
|
|
|
|
/* prepare TX queue */
|
|
for (i = 0; i < 3; i++)
|
|
uk0->tx_queue_tail[i] = &uk0->tx_queue[i];
|
|
|
|
/* prepare TX */
|
|
tx->phase_step = SYMBOL_RATE / (double)samplerate;
|
|
|
|
tx->scramble = ~0;
|
|
tx->mms43_column = 0;
|
|
|
|
/* prepare RX */
|
|
rx->window_length = RX_SINC_WINDOW * RX_OVERSAMPLING;
|
|
rx->sinc = calloc(rx->window_length, sizeof(*rx->sinc));
|
|
if (!rx->sinc)
|
|
return NULL;
|
|
rx->window = calloc(rx->window_length, sizeof(*rx->window));
|
|
if (!rx->window)
|
|
return NULL;
|
|
|
|
for (i = 0; i < rx->window_length; i++) {
|
|
x = M_PI * (double)(i - (rx->window_length >> 1)) / (double)RX_OVERSAMPLING;
|
|
if (fabs(x) < 0.000001)
|
|
l = 1;
|
|
else
|
|
l = sin(x)/x;
|
|
l *= 0.54 - 0.46 * cos(2.0 * M_PI * (double)i / (double)rx->window_length);
|
|
rx->sinc[i] = l;
|
|
// puts(debug_amplitude(rx->sinc[i]));
|
|
}
|
|
|
|
rx->phase_step = 2.0 * M_PI * SYMBOL_RATE / ((double)samplerate * (double)RX_OVERSAMPLING);
|
|
rx->ms_per_sample = 1000.0 / ((double)samplerate * (double)RX_OVERSAMPLING);
|
|
/* use fourth order (2 iter) filter, since it is as fast as second order (1 iter) filter */
|
|
/* symbol rate divided by 10000 equals 100 ppm tolerance */
|
|
iir_lowpass_init(&rx->lp[0], RX_CLOCK_FILTER, samplerate * RX_OVERSAMPLING, 2);
|
|
iir_lowpass_init(&rx->lp[1], RX_CLOCK_FILTER, samplerate * RX_OVERSAMPLING, 2);
|
|
|
|
rx->descramble = 0;
|
|
|
|
/* recording for debugging */
|
|
#ifdef DEBUG_RECORD_RX
|
|
#ifdef DEBUG_RECORD_UPSAMPLE
|
|
wave_create_record(&rx->rec, "uk0.wav", samplerate * RX_OVERSAMPLING, 1, 1.0);
|
|
#else
|
|
wave_create_record(&rx->rec, "uk0.wav", samplerate, 1, 1.0);
|
|
#endif
|
|
#endif
|
|
|
|
/* measurement */
|
|
display_wave_init(&uk0->dispwav, samplerate, uk0->name);
|
|
uk0->measure_level_interval = (int)(SYMBOL_RATE / 30.0);
|
|
uk0->measure_frame_interval = FRAME_RATE / 10;
|
|
display_measurements_init(&uk0->dispmeas, samplerate, uk0->name);
|
|
uk0->dmp_level = display_measurements_add(&uk0->dispmeas, "RX Level", "%.0f %%", DISPLAY_MEAS_PEAK, DISPLAY_MEAS_LEFT, 0.0, 100.0, 100.0);
|
|
uk0->dmp_level_error = display_measurements_add(&uk0->dispmeas, "RX Level Error", "%.0f %%", DISPLAY_MEAS_LAST, DISPLAY_MEAS_LEFT, 0.0, 100.0, 100.0);
|
|
|
|
PDEBUG_CHAN(DUK0, DEBUG_INFO, "UK0 has been created.\n");
|
|
|
|
return uk0;
|
|
}
|
|
|
|
/* close UK0 Interface */
|
|
void uk0_destroy(uk0_t *uk0)
|
|
{
|
|
uk0_rx_t *rx = &uk0->rx;
|
|
int i;
|
|
|
|
PDEBUG_CHAN(DUK0, DEBUG_INFO, "UK0 is beeing destroyed!\n");
|
|
|
|
if (rx->sinc) {
|
|
free(rx->sinc);
|
|
rx->sinc = NULL;
|
|
}
|
|
if (rx->window) {
|
|
free(rx->window);
|
|
rx->window = NULL;
|
|
}
|
|
|
|
for (i = 0; i < 3; i++)
|
|
uk0_tx_flush(uk0, i + 1);
|
|
|
|
#ifdef DEBUG_RECORD_RX
|
|
wave_destroy_record(&rx->rec);
|
|
#endif
|
|
|
|
free(uk0);
|
|
}
|
|
|
|
/*
|
|
* queue and dejitter
|
|
*/
|
|
|
|
void uk0_tx_flush(uk0_t *uk0, int channel)
|
|
{
|
|
struct tx_queue *msg;
|
|
int i;
|
|
|
|
if (channel < 1 || channel > 3)
|
|
return;
|
|
i = channel - 1;
|
|
|
|
while (uk0->tx_queue[i]) {
|
|
msg = uk0->tx_queue[i];
|
|
uk0->tx_queue[i] = msg->next;
|
|
free(msg);
|
|
}
|
|
uk0->tx_queue_tail[i] = &uk0->tx_queue[i];
|
|
uk0->tx_queue_size[i] = 0;
|
|
}
|
|
|
|
void uk0_tx_enqueue(uk0_t *uk0, int channel, uint8_t *data, int length)
|
|
{
|
|
struct tx_queue *msg;
|
|
int i;
|
|
|
|
if (length < 1)
|
|
return;
|
|
|
|
if (channel < 1 || channel > 3)
|
|
return;
|
|
i = channel - 1;
|
|
|
|
if (uk0->tx_queue_size[i] > 800) {
|
|
PDEBUG_CHAN(DUK0, DEBUG_INFO, "Dropping data on channel %d, due to overfilled transmit buffer.\n", channel);
|
|
return;
|
|
}
|
|
|
|
msg = calloc(1, sizeof(*msg) + length);
|
|
memcpy(msg->data, data, length);
|
|
msg->length = length;
|
|
*uk0->tx_queue_tail[i] = msg;
|
|
uk0->tx_queue_tail[i] = &msg->next;
|
|
uk0->tx_queue_size[i] += length;
|
|
}
|
|
|
|
int uk0_tx_dequeue(uk0_t *uk0, int channel, uint8_t *data, int size)
|
|
{
|
|
struct tx_queue *msg;
|
|
int i, length;
|
|
|
|
if (channel < 1 || channel > 3)
|
|
return 0;
|
|
i = channel - 1;
|
|
|
|
if (!uk0->tx_queue[i])
|
|
return 0;
|
|
|
|
msg = uk0->tx_queue[i];
|
|
if (msg->length <= size) {
|
|
memcpy(data, msg->data, msg->length);
|
|
length = msg->length;
|
|
} else {
|
|
length = 0;
|
|
}
|
|
uk0->tx_queue_size[i] -= msg->length;
|
|
uk0->tx_queue[i] = msg->next;
|
|
free(msg);
|
|
if (!uk0->tx_queue[i])
|
|
uk0->tx_queue_tail[i] = &uk0->tx_queue[i];
|
|
|
|
return length;
|
|
}
|
|
|
|
/*
|
|
* channel multiplexing
|
|
*/
|
|
|
|
/* inject B+B+D channels from frame */
|
|
static void mux_frame(uk0_t *uk0, uint8_t *data)
|
|
{
|
|
uk0_tx_t *tx = &uk0->tx;
|
|
uint8_t *B1 = tx->B1 + tx->b_count;
|
|
uint8_t *B2 = tx->B2 + tx->b_count;
|
|
uint8_t *D = tx->D + tx->d_count;
|
|
|
|
if (tx->b_count == 0) {
|
|
uk0_send(uk0, 1, tx->B1, TX_NUM_B);
|
|
uk0_send(uk0, 2, tx->B2, TX_NUM_B);
|
|
#ifdef HDLC_LOOPBACK
|
|
uk0_receive(uk0, 1, tx->B1, TX_NUM_B);
|
|
uk0_receive(uk0, 2, tx->B2, TX_NUM_B);
|
|
#endif
|
|
}
|
|
if (tx->d_count == 0) {
|
|
uk0_send(uk0, 3, tx->D, TX_NUM_D);
|
|
#ifdef HDLC_LOOPBACK
|
|
printf("hdlc encoded data: %s\n", debug_hex(tx->D, TX_NUM_D));
|
|
uk0_receive(uk0, 3, tx->D, TX_NUM_D);
|
|
#endif
|
|
}
|
|
|
|
/* B1(1..8) B2(1..8) D(1..2) */
|
|
data[0] = B1[0];
|
|
data[1] = B2[0];
|
|
data[2] = D[0] & 0x03;
|
|
|
|
/* B1(1..8) B2(1..8) D(3..4) */
|
|
data[2] |= B1[1] << 2;
|
|
data[3] = B1[1] >> 6;
|
|
data[3] |= B2[1] << 2;
|
|
data[4] = B2[1] >> 6;
|
|
data[4] |= D[0] & 0x0c;
|
|
|
|
/* B1(1..8) B2(1..8) D(5..6) */
|
|
data[4] |= B1[2] << 4;
|
|
data[5] = B1[2] >> 4;
|
|
data[5] |= B2[2] << 4;
|
|
data[6] = B2[2] >> 4;
|
|
data[6] |= D[0] & 0x30;
|
|
|
|
/* B1(1..8) B2(1..8) D(7..8) */
|
|
data[6] |= B1[3] << 6;
|
|
data[7] = B1[3] >> 2;
|
|
data[7] |= B2[3] << 6;
|
|
data[8] = B2[3] >> 2;
|
|
data[8] |= D[0] & 0xc0;
|
|
|
|
/* B1(1..8) B2(1..8) D(1..2) */
|
|
data[9] = B1[4];
|
|
data[10] = B2[4];
|
|
data[11] = D[1] & 0x03;
|
|
|
|
/* B1(1..8) B2(1..8) D(3..4) */
|
|
data[11] |= B1[5] << 2;
|
|
data[12] = B1[5] >> 6;
|
|
data[12] |= B2[5] << 2;
|
|
data[13] = B2[5] >> 6;
|
|
data[13] |= D[1] & 0x0c;
|
|
|
|
/* B1(1..8) B2(1..8) D(5..6) */
|
|
data[13] |= B1[6] << 4;
|
|
data[14] = B1[6] >> 4;
|
|
data[14] |= B2[6] << 4;
|
|
data[15] = B2[6] >> 4;
|
|
data[15] |= D[1] & 0x30;
|
|
|
|
/* B1(1..8) B2(1..8) D(7..8) */
|
|
data[15] |= B1[7] << 6;
|
|
data[16] = B1[7] >> 2;
|
|
data[16] |= B2[7] << 6;
|
|
data[17] = B2[7] >> 2;
|
|
data[17] |= D[1] & 0xc0;
|
|
|
|
tx->b_count += 8;
|
|
tx->d_count += 2;
|
|
if (tx->b_count == TX_NUM_B)
|
|
tx->b_count = 0;
|
|
if (tx->d_count == TX_NUM_D)
|
|
tx->d_count = 0;
|
|
}
|
|
|
|
/* extract B+B+D channels from frame */
|
|
static void demux_frame(uk0_t *uk0, uint8_t *data)
|
|
{
|
|
uk0_rx_t *rx = &uk0->rx;
|
|
uint8_t *B1 = rx->B1 + rx->b_count;
|
|
uint8_t *B2 = rx->B2 + rx->b_count;
|
|
uint8_t *D = rx->D + rx->d_count;
|
|
|
|
/* B1(1..8) B2(1..8) D(1..2) */
|
|
B1[0] = data[0];
|
|
B2[0] = data[1];
|
|
D[0] = data[2] & 0x03;
|
|
|
|
/* B1(1..8) B2(1..8) D(3..4) */
|
|
B1[1] = data[2] >> 2;
|
|
B1[1] |= data[3] << 6;
|
|
B2[1] = data[3] >> 2;
|
|
B2[1] |= data[4] << 6;
|
|
D[0] |= data[4] & 0x0c;
|
|
|
|
/* B1(1..8) B2(1..8) D(5..6) */
|
|
B1[2] = data[4] >> 4;
|
|
B1[2] |= data[5] << 4;
|
|
B2[2] = data[5] >> 4;
|
|
B2[2] |= data[6] << 4;
|
|
D[0] |= data[6] & 0x30;
|
|
|
|
/* B1(1..8) B2(1..8) D(7..8) */
|
|
B1[3] = data[6] >> 6;
|
|
B1[3] |= data[7] << 2;
|
|
B2[3] = data[7] >> 6;
|
|
B2[3] |= data[8] << 2;
|
|
D[0] |= data[8] & 0xc0;
|
|
|
|
/* B1(1..8) B2(1..8) D(1..2) */
|
|
B1[4] = data[9];
|
|
B2[4] = data[10];
|
|
D[1] = data[11] & 0x03;
|
|
|
|
/* B1(1..8) B2(1..8) D(3..4) */
|
|
B1[5] = data[11] >> 2;
|
|
B1[5] |= data[12] << 6;
|
|
B2[5] = data[12] >> 2;
|
|
B2[5] |= data[13] << 6;
|
|
D[1] |= data[13] & 0x0c;
|
|
|
|
/* B1(1..8) B2(1..8) D(5..6) */
|
|
B1[6] = data[13] >> 4;
|
|
B1[6] |= data[14] << 4;
|
|
B2[6] = data[14] >> 4;
|
|
B2[6] |= data[15] << 4;
|
|
D[1] |= data[15] & 0x30;
|
|
|
|
/* B1(1..8) B2(1..8) D(7..8) */
|
|
B1[7] = data[15] >> 6;
|
|
B1[7] |= data[16] << 2;
|
|
B2[7] = data[16] >> 6;
|
|
B2[7] |= data[17] << 2;
|
|
D[1] |= data[17] & 0xc0;
|
|
|
|
rx->b_count += 8;
|
|
rx->d_count += 2;
|
|
if (rx->b_count == RX_NUM_B) {
|
|
rx->b_count = 0;
|
|
#ifndef HDLC_LOOPBACK
|
|
uk0_receive(uk0, 1, rx->B1, RX_NUM_B);
|
|
uk0_receive(uk0, 2, rx->B2, RX_NUM_B);
|
|
#endif
|
|
}
|
|
if (rx->d_count == RX_NUM_D) {
|
|
rx->d_count = 0;
|
|
#ifndef HDLC_LOOPBACK
|
|
uk0_receive(uk0, 3, rx->D, RX_NUM_D);
|
|
#endif
|
|
}
|
|
}
|
|
|
|
/*
|
|
* LT state machine
|
|
*/
|
|
|
|
static const char *uk0_state_names[] = {
|
|
"NULL",
|
|
"LT 1.1",
|
|
"LT 1.2",
|
|
"LT 1.3",
|
|
"LT 1.4",
|
|
"LT 1.5",
|
|
"LT 1.6",
|
|
"LT 1.7",
|
|
"LT 1.8",
|
|
};
|
|
|
|
static const char *uk0_event_names[] = {
|
|
"DISABLE",
|
|
"ENABLE",
|
|
"DEACTIVATE",
|
|
"ACTIVATE",
|
|
"TIMEOUT",
|
|
"U0",
|
|
"U1W",
|
|
"U1A",
|
|
"U1",
|
|
"U3",
|
|
"U5",
|
|
"SP",
|
|
"ANY",
|
|
};
|
|
|
|
/* sub function to change state */
|
|
static void uk0_new_state(uk0_t *uk0, enum uk0_state new_state)
|
|
{
|
|
PDEBUG_CHAN(DUK0, DEBUG_INFO, "Changing call state from %s to %s.\n", uk0_state_names[uk0->state], uk0_state_names[new_state]);
|
|
uk0->state = new_state;
|
|
}
|
|
|
|
/* enable interface */
|
|
void uk0_enable(uk0_t *uk0)
|
|
{
|
|
PDEBUG_CHAN(DUK0, DEBUG_INFO, "Enable interface.\n");
|
|
hdlc_tx_init(&uk0->hdlc_tx[2], uk0, 3, HDLC_MODE_HDLC);
|
|
hdlc_rx_init(&uk0->hdlc_rx[2], uk0, 3, HDLC_MODE_HDLC);
|
|
uk0_new_state(uk0, UK0_STATE_LT_1_1);
|
|
}
|
|
|
|
/* completely disable interface */
|
|
void uk0_disable(uk0_t *uk0)
|
|
{
|
|
int i;
|
|
PDEBUG_CHAN(DUK0, DEBUG_INFO, "Disable interface.\n");
|
|
for (i = 0; i < 3; i++) {
|
|
hdlc_tx_init(&uk0->hdlc_tx[i], uk0, i + 1, HDLC_MODE_OFF);
|
|
hdlc_rx_init(&uk0->hdlc_rx[i], uk0, i + 1, HDLC_MODE_OFF);
|
|
uk0_tx_flush(uk0, i + 1);
|
|
}
|
|
uk0_new_state(uk0, UK0_STATE_NULL);
|
|
}
|
|
|
|
/* sub function to activate from upper layer */
|
|
void uk0_activate(uk0_t *uk0)
|
|
{
|
|
PDEBUG_CHAN(DUK0, DEBUG_INFO, "Activation request by LT, sending U2W to wake up NT.\n");
|
|
uk0_new_state(uk0, UK0_STATE_LT_1_3);
|
|
/* start timer for resending U1W */
|
|
uk0->timer = uk0->ti1;
|
|
}
|
|
|
|
/* sub function to activate from remote NT */
|
|
void uk0_u1w_indication(uk0_t *uk0)
|
|
{
|
|
PDEBUG_CHAN(DUK0, DEBUG_INFO, "Receive U1W activation request by NT, sending U2W to NT to acknowledge.\n");
|
|
uk0_new_state(uk0, UK0_STATE_LT_1_2);
|
|
uk0->timer = uk0->ti1;
|
|
/* we don't wait for TI1 (7ms), because we have more delay
|
|
* we send U1W, then send U0 for 7ms, then send U2 until timeout
|
|
* then we transit to LT 1.4
|
|
*/
|
|
}
|
|
|
|
/* sub function to handle TI1 timeout */
|
|
void uk0_timeout_ti1(uk0_t *uk0)
|
|
{
|
|
/* after sending LT U2W, transit to LT 1.4 */
|
|
PDEBUG_CHAN(DUK0, DEBUG_INFO, "Done sending U2W, wait for framing or wake up signal again.\n");
|
|
uk0->timer = uk0->ti1;
|
|
uk0_new_state(uk0, UK0_STATE_LT_1_1);
|
|
}
|
|
|
|
/* sub function to transit to LT 1.4 */
|
|
void uk0_got_framing(uk0_t *uk0)
|
|
{
|
|
PDEBUG_CHAN(DUK0, DEBUG_INFO, "We got some framing now.\n");
|
|
uk0_new_state(uk0, UK0_STATE_LT_1_4);
|
|
}
|
|
|
|
/* sub function to handle U1 */
|
|
void uk0_u1(uk0_t *uk0)
|
|
{
|
|
PDEBUG_CHAN(DUK0, DEBUG_INFO, "Got U1 from NT.\n");
|
|
uk0_new_state(uk0, UK0_STATE_LT_1_5);
|
|
}
|
|
|
|
/* sub function to handle U3 */
|
|
void uk0_u3(uk0_t *uk0)
|
|
{
|
|
PDEBUG_CHAN(DUK0, DEBUG_INFO, "Got U3 from NT, send U4H.\n");
|
|
uk0_new_state(uk0, UK0_STATE_LT_1_6);
|
|
uk0->timer = uk0->ti2; /* one ms */
|
|
}
|
|
|
|
/* sub function to handle TI2 timeout */
|
|
void uk0_timeout_ti2(uk0_t *uk0)
|
|
{
|
|
PDEBUG_CHAN(DUK0, DEBUG_INFO, "Done sending U4H, activation complete.\n");
|
|
uk0_new_state(uk0, UK0_STATE_LT_1_7);
|
|
ph_socket_tx_msg(&uk0->ph_socket, 3, PH_PRIM_ACT_IND, NULL, 0);
|
|
}
|
|
|
|
/* sub function to handle deactivation from NT */
|
|
void uk0_u0(uk0_t *uk0)
|
|
{
|
|
PDEBUG_CHAN(DUK0, DEBUG_INFO, "Received U0 from NT, send U0.\n");
|
|
uk0_new_state(uk0, UK0_STATE_LT_1_1);
|
|
ph_socket_tx_msg(&uk0->ph_socket, 3, PH_PRIM_DACT_IND, NULL, 0);
|
|
}
|
|
|
|
/* sub function to handle deactivation from upper layer */
|
|
void uk0_deactivate(uk0_t *uk0)
|
|
{
|
|
PDEBUG_CHAN(DUK0, DEBUG_INFO, "We deactivate, send U0.\n");
|
|
if (uk0->state == UK0_STATE_LT_1_4
|
|
|| uk0->state == UK0_STATE_LT_1_5
|
|
|| uk0->state == UK0_STATE_LT_1_6
|
|
|| uk0->state == UK0_STATE_LT_1_7)
|
|
uk0_new_state(uk0, UK0_STATE_LT_1_8);
|
|
else
|
|
uk0_new_state(uk0, UK0_STATE_LT_1_1);
|
|
}
|
|
|
|
/* sub function to handle deactivation confirm from NT */
|
|
void uk0_deactivated(uk0_t *uk0)
|
|
{
|
|
PDEBUG_CHAN(DUK0, DEBUG_INFO, "Deactivation complete.\n");
|
|
uk0_new_state(uk0, UK0_STATE_LT_1_1);
|
|
ph_socket_tx_msg(&uk0->ph_socket, 3, PH_PRIM_DACT_IND, NULL, 0);
|
|
}
|
|
|
|
/* we confirm activation, if already */
|
|
void uk0_already_activated(uk0_t *uk0)
|
|
{
|
|
ph_socket_tx_msg(&uk0->ph_socket, 3, PH_PRIM_ACT_IND, NULL, 0);
|
|
}
|
|
|
|
#define SBIT(a) (1 << a)
|
|
#define ALL_STATES (~0)
|
|
|
|
/* UK0 state machine */
|
|
static struct statemachine {
|
|
uint32_t states;
|
|
enum uk0_event event;
|
|
void (*action)(uk0_t *uk0);
|
|
} statemachine_list[] = {
|
|
/* enable und disable */
|
|
{SBIT(UK0_STATE_NULL),
|
|
UK0_EVENT_ENABLE, uk0_enable},
|
|
{SBIT(UK0_STATE_NULL),
|
|
UK0_EVENT_ANY, NULL},
|
|
{ALL_STATES,
|
|
UK0_EVENT_DISABLE, uk0_disable},
|
|
|
|
/* deactivation */
|
|
{ALL_STATES,
|
|
UK0_EVENT_DEACTIVATE, uk0_deactivate},
|
|
{SBIT(UK0_STATE_LT_1_8),
|
|
UK0_EVENT_U0, uk0_deactivated},
|
|
{SBIT(UK0_STATE_LT_1_8),
|
|
UK0_EVENT_ANY, NULL},
|
|
|
|
/* activate from upper layer */
|
|
{SBIT(UK0_STATE_LT_1_1),
|
|
UK0_EVENT_ACTIVATE, uk0_activate},
|
|
{SBIT(UK0_STATE_LT_1_7),
|
|
UK0_EVENT_ACTIVATE, uk0_already_activated},
|
|
{SBIT(UK0_STATE_LT_1_3),
|
|
UK0_EVENT_TIMEOUT, uk0_activate},
|
|
|
|
/* activate from NT */
|
|
{SBIT(UK0_STATE_LT_1_1),
|
|
UK0_EVENT_U1W, uk0_u1w_indication},
|
|
{SBIT(UK0_STATE_LT_1_2),
|
|
UK0_EVENT_TIMEOUT, uk0_timeout_ti1},
|
|
|
|
/* when we got framing we go to LT 1.4 */
|
|
{SBIT(UK0_STATE_LT_1_2) | SBIT(UK0_STATE_LT_1_3),
|
|
UK0_EVENT_U1A, uk0_got_framing},
|
|
{SBIT(UK0_STATE_LT_1_2) | SBIT(UK0_STATE_LT_1_3),
|
|
UK0_EVENT_U1, uk0_got_framing},
|
|
{SBIT(UK0_STATE_LT_1_2) | SBIT(UK0_STATE_LT_1_3),
|
|
UK0_EVENT_U3, uk0_got_framing},
|
|
{SBIT(UK0_STATE_LT_1_2) | SBIT(UK0_STATE_LT_1_3),
|
|
UK0_EVENT_ANY, NULL},
|
|
|
|
/* ignore any other signal in idle state */
|
|
{SBIT(UK0_STATE_LT_1_1),
|
|
UK0_EVENT_ANY, NULL},
|
|
|
|
/* wait for sync or fall back on wake up */
|
|
{SBIT(UK0_STATE_LT_1_4),
|
|
UK0_EVENT_U0, uk0_activate},
|
|
{SBIT(UK0_STATE_LT_1_4),
|
|
UK0_EVENT_U1W, uk0_u1w_indication},
|
|
{SBIT(UK0_STATE_LT_1_4),
|
|
UK0_EVENT_U1A, NULL},
|
|
|
|
/* we complete framing */
|
|
{SBIT(UK0_STATE_LT_1_4),
|
|
UK0_EVENT_U1, uk0_u1},
|
|
{SBIT(UK0_STATE_LT_1_4),
|
|
UK0_EVENT_U3, uk0_u3},
|
|
{SBIT(UK0_STATE_LT_1_5),
|
|
UK0_EVENT_U0, uk0_u0},
|
|
{SBIT(UK0_STATE_LT_1_5),
|
|
UK0_EVENT_U3, uk0_u3},
|
|
{SBIT(UK0_STATE_LT_1_5),
|
|
UK0_EVENT_ANY, NULL},
|
|
{SBIT(UK0_STATE_LT_1_6),
|
|
UK0_EVENT_U0, uk0_u0},
|
|
{SBIT(UK0_STATE_LT_1_6),
|
|
UK0_EVENT_TIMEOUT, uk0_timeout_ti2},
|
|
{SBIT(UK0_STATE_LT_1_6),
|
|
UK0_EVENT_ANY, NULL},
|
|
|
|
/* framing complete */
|
|
{SBIT(UK0_STATE_LT_1_7),
|
|
UK0_EVENT_U0, uk0_u0},
|
|
{SBIT(UK0_STATE_LT_1_7),
|
|
UK0_EVENT_ANY, NULL},
|
|
};
|
|
|
|
#define STATEMACHINE_LEN \
|
|
(sizeof(statemachine_list) / sizeof(struct statemachine))
|
|
|
|
/* handle event on state machine */
|
|
void uk0_handle_event(uk0_t *uk0, enum uk0_event event)
|
|
{
|
|
int i;
|
|
|
|
/* find action for event in current state */
|
|
for (i = 0; i < (int)STATEMACHINE_LEN; i++) {
|
|
if ((event == statemachine_list[i].event
|
|
|| statemachine_list[i].event == UK0_EVENT_ANY)
|
|
&& ((1 << uk0->state) & statemachine_list[i].states)) {
|
|
break;
|
|
}
|
|
}
|
|
if (i == STATEMACHINE_LEN) {
|
|
PDEBUG_CHAN(DUK0, DEBUG_INFO, "Event %s unhandled at state %s\n",
|
|
uk0_event_names[event], uk0_state_names[uk0->state]);
|
|
return;
|
|
}
|
|
|
|
if (statemachine_list[i].action) {
|
|
PDEBUG_CHAN(DUK0, DEBUG_INFO, "Event message %s at state %s\n",
|
|
uk0_event_names[event], uk0_state_names[uk0->state]);
|
|
statemachine_list[i].action(uk0);
|
|
}
|
|
}
|
|
|
|
void ph_socket_rx_msg(ph_socket_t *s, int channel, uint8_t prim, uint8_t *data, int length)
|
|
{
|
|
uk0_t *uk0 = (uk0_t *)s->priv;
|
|
hdlc_tx_t *hdlc_tx = NULL;
|
|
hdlc_rx_t *hdlc_rx = NULL;
|
|
int hdlc = 0;
|
|
|
|
/* no channel specific messages */
|
|
switch (prim) {
|
|
case PH_PRIM_CTRL_REQ:
|
|
if (length >= 1) {
|
|
switch (data[0]) {
|
|
case PH_CTRL_ENABLE:
|
|
uk0_handle_event(uk0, UK0_EVENT_ENABLE);
|
|
return;
|
|
case PH_CTRL_DISABLE:
|
|
uk0_handle_event(uk0, UK0_EVENT_DISABLE);
|
|
return;
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
|
|
if (channel >= 1 && channel <= 3) {
|
|
hdlc_tx = &uk0->hdlc_tx[channel - 1];
|
|
hdlc_rx = &uk0->hdlc_rx[channel - 1];
|
|
} else {
|
|
PDEBUG_CHAN(DPH, DEBUG_ERROR, "Rejecting message 0x%02x for unsupported channel %d!\n", prim, channel);
|
|
return;
|
|
}
|
|
|
|
switch (prim) {
|
|
case PH_PRIM_DATA_REQ:
|
|
uk0_tx_enqueue(uk0, channel, data, length);
|
|
break;
|
|
case PH_PRIM_ACT_REQ:
|
|
if (length >= 1 && data[0] == PH_MODE_HDLC)
|
|
hdlc = 1;
|
|
if (channel == 3) {
|
|
/* reset transmitter to clean up residual data FIXME: somewhere else? */
|
|
hdlc_tx_init(hdlc_tx, uk0, channel, HDLC_MODE_HDLC);
|
|
uk0_handle_event(uk0, UK0_EVENT_ACTIVATE);
|
|
break;
|
|
}
|
|
hdlc_tx_init(hdlc_tx, uk0, channel, (hdlc) ? HDLC_MODE_HDLC : HDLC_MODE_TRANS);
|
|
hdlc_rx_init(hdlc_rx, uk0, channel, (hdlc) ? HDLC_MODE_HDLC : HDLC_MODE_TRANS);
|
|
/* confirm */
|
|
ph_socket_tx_msg(&uk0->ph_socket, channel, PH_PRIM_ACT_IND, NULL, 0);
|
|
break;
|
|
case PH_PRIM_DACT_REQ:
|
|
uk0_tx_flush(uk0, channel);
|
|
if (channel == 3) {
|
|
uk0_handle_event(uk0, UK0_EVENT_DEACTIVATE);
|
|
break;
|
|
}
|
|
hdlc_tx_init(hdlc_tx, uk0, channel, HDLC_MODE_OFF);
|
|
hdlc_rx_init(hdlc_rx, uk0, channel, HDLC_MODE_OFF);
|
|
/* confirm */
|
|
ph_socket_tx_msg(&uk0->ph_socket, channel, PH_PRIM_DACT_IND, NULL, 0);
|
|
break;
|
|
default:
|
|
PDEBUG_CHAN(DPH, DEBUG_ERROR, "Rejecting unknown message 0x%02x!\n", prim);
|
|
}
|
|
}
|
|
|
|
/*
|
|
* UK0 frame transcoding
|
|
*/
|
|
|
|
static int encode_frame(uk0_tx_t *tx, uint64_t *buffer, uint8_t *data, uint32_t sync, uint8_t m)
|
|
{
|
|
int i, j;
|
|
uint8_t bin1, bin2, in, out = 0;
|
|
uint64_t code;
|
|
uint8_t column = tx->mms43_column;
|
|
|
|
/* scramble */
|
|
#ifdef DEBUG_SCRAMBLER
|
|
printf("TX scramble ");
|
|
#endif
|
|
for (i = 0; i < 18; i++) {
|
|
if (m)
|
|
in = 0xff;
|
|
else
|
|
in = data[i];
|
|
#ifdef DEBUG_SCRAMBLER
|
|
printf(" %02x", in);
|
|
#endif
|
|
for (j = 0; j < 8; j++) {
|
|
out >>= 1;
|
|
out |= ((tx->scramble >> 23) ^ (tx->scramble >> 5) ^ in) << 7;
|
|
tx->scramble |= out >> 7;
|
|
tx->scramble <<= 1;
|
|
in >>= 1;
|
|
}
|
|
data[i] = out;
|
|
}
|
|
#ifdef DEBUG_SCRAMBLER
|
|
printf("\n");
|
|
#endif
|
|
|
|
/* convert binary to symbols in correct order */
|
|
bin1 = data[0] & 0xf; bin2 = data[0] >> 4;
|
|
code = mms43_encode[bin1][column]; column = code >> 6;
|
|
buffer[0] = code & 63;
|
|
code = mms43_encode[bin2][column]; column = code >> 6;
|
|
buffer[0] |= (code & 63) << 6;
|
|
bin1 = data[1] & 0xf; bin2 = data[1] >> 4;
|
|
code = mms43_encode[bin1][column]; column = code >> 6;
|
|
buffer[0] |= (code & 63) << 12;
|
|
code = mms43_encode[bin2][column]; column = code >> 6;
|
|
buffer[0] |= (code & 63) << 18;
|
|
bin1 = data[2] & 0xf; bin2 = data[2] >> 4;
|
|
code = mms43_encode[bin1][column]; column = code >> 6;
|
|
buffer[0] |= (code & 63) << 24;
|
|
code = mms43_encode[bin2][column]; column = code >> 6;
|
|
buffer[0] |= (code & 63) << 30;
|
|
bin1 = data[3] & 0xf; bin2 = data[3] >> 4;
|
|
code = mms43_encode[bin1][column]; column = code >> 6;
|
|
buffer[0] |= (code & 63) << 36;
|
|
code = mms43_encode[bin2][column]; column = code >> 6;
|
|
buffer[0] |= (code & 63) << 42;
|
|
bin1 = data[4] & 0xf; bin2 = data[4] >> 4;
|
|
code = mms43_encode[bin1][column]; column = code >> 6;
|
|
buffer[0] |= (code & 63) << 48;
|
|
code = mms43_encode[bin2][column]; column = code >> 6;
|
|
buffer[0] |= (code & 63) << 54;
|
|
bin1 = data[5] & 0xf; bin2 = data[5] >> 4;
|
|
code = mms43_encode[bin1][column]; column = code >> 6;
|
|
buffer[0] |= (code & 63) << 60;
|
|
buffer[1] = (code & 63) >> 4;
|
|
code = mms43_encode[bin2][column]; column = code >> 6;
|
|
buffer[1] |= (code & 63) << 2;
|
|
bin1 = data[6] & 0xf; bin2 = data[6] >> 4;
|
|
code = mms43_encode[bin1][column]; column = code >> 6;
|
|
buffer[1] |= (code & 63) << 8;
|
|
code = mms43_encode[bin2][column]; column = code >> 6;
|
|
buffer[1] |= (code & 63) << 14;
|
|
bin1 = data[7] & 0xf; bin2 = data[7] >> 4;
|
|
code = mms43_encode[bin1][column]; column = code >> 6;
|
|
buffer[1] |= (code & 63) << 20;
|
|
code = mms43_encode[bin2][column]; column = code >> 6;
|
|
buffer[1] |= (code & 63) << 26;
|
|
bin1 = data[8] & 0xf; bin2 = data[8] >> 4;
|
|
code = mms43_encode[bin1][column]; column = code >> 6;
|
|
buffer[1] |= (code & 63) << 32;
|
|
code = mms43_encode[bin2][column]; column = code >> 6;
|
|
buffer[1] |= (code & 63) << 38;
|
|
bin1 = data[9] & 0xf; bin2 = data[9] >> 4;
|
|
code = mms43_encode[bin1][column]; column = code >> 6;
|
|
buffer[1] |= (code & 63) << 44;
|
|
code = mms43_encode[bin2][column]; column = code >> 6;
|
|
buffer[1] |= (code & 63) << 50;
|
|
bin1 = data[10] & 0xf; bin2 = data[10] >> 4;
|
|
code = mms43_encode[bin1][column]; column = code >> 6;
|
|
buffer[1] |= (code & 63) << 56;
|
|
code = mms43_encode[bin2][column]; column = code >> 6;
|
|
buffer[1] |= (code & 63) << 62;
|
|
buffer[2] = (code & 63) >> 2;
|
|
bin1 = data[11] & 0xf; bin2 = data[11] >> 4;
|
|
code = mms43_encode[bin1][column]; column = code >> 6;
|
|
buffer[2] |= (code & 63) << 4;
|
|
code = mms43_encode[bin2][column]; column = code >> 6;
|
|
buffer[2] |= (code & 63) << 10;
|
|
bin1 = data[12] & 0xf; bin2 = data[12] >> 4;
|
|
code = mms43_encode[bin1][column]; column = code >> 6;
|
|
buffer[2] |= (code & 63) << 16;
|
|
code = mms43_encode[bin2][column]; column = code >> 6;
|
|
buffer[2] |= (code & 63) << 22;
|
|
bin1 = data[13] & 0xf; bin2 = data[13] >> 4;
|
|
code = mms43_encode[bin1][column]; column = code >> 6;
|
|
buffer[2] |= (code & 63) << 28;
|
|
code = mms43_encode[bin2][column]; column = code >> 6;
|
|
buffer[2] |= (code & 63) << 34;
|
|
buffer[2] |= (uint64_t)(m & 1) << 40;
|
|
bin1 = data[14] & 0xf; bin2 = data[14] >> 4;
|
|
code = mms43_encode[bin1][column]; column = code >> 6;
|
|
buffer[2] |= (code & 63) << 42;
|
|
code = mms43_encode[bin2][column]; column = code >> 6;
|
|
buffer[2] |= (code & 63) << 48;
|
|
bin1 = data[15] & 0xf; bin2 = data[15] >> 4;
|
|
code = mms43_encode[bin1][column]; column = code >> 6;
|
|
buffer[2] |= (code & 63) << 54;
|
|
code = mms43_encode[bin2][column]; column = code >> 6;
|
|
buffer[2] |= (code & 63) << 60;
|
|
buffer[3] = (code & 63) >> 4;
|
|
bin1 = data[16] & 0xf; bin2 = data[16] >> 4;
|
|
code = mms43_encode[bin1][column]; column = code >> 6;
|
|
buffer[3] |= (code & 63) << 2;
|
|
code = mms43_encode[bin2][column]; column = code >> 6;
|
|
buffer[3] |= (code & 63) << 8;
|
|
bin1 = data[17] & 0xf; bin2 = data[17] >> 4;
|
|
code = mms43_encode[bin1][column]; column = code >> 6;
|
|
buffer[3] |= (code & 63) << 14;
|
|
code = mms43_encode[bin2][column]; column = code >> 6;
|
|
buffer[3] |= (code & 63) << 20;
|
|
buffer[3] |= (uint64_t)(sync & SYNC_MASK) << 26;
|
|
|
|
#ifdef DEBUG_FRAME
|
|
int tern, s;
|
|
printf("TX frame: ");
|
|
for (s = 0; s < 120; s++) {
|
|
tern = (buffer[s >> 5] >> (2*(s&31))) & 3;
|
|
switch (tern) {
|
|
case 1: printf("+"); break;
|
|
case 2: printf("-"); break;
|
|
case 0: printf("0"); break;
|
|
case 3: printf("?"); break;
|
|
}
|
|
}
|
|
printf("\n");
|
|
printf("sync: ");
|
|
for (s = 0; s < 11; s++) {
|
|
switch ((sync) >> (s*2) & 3) {
|
|
case 1: printf("+"); break;
|
|
case 2: printf("-"); break;
|
|
case 0: printf("0"); break;
|
|
case 3: printf("?"); break;
|
|
}
|
|
}
|
|
printf(" M: ");
|
|
switch ((m)) {
|
|
case 1: printf("+"); break;
|
|
case 2: printf("-"); break;
|
|
case 0: printf("0"); break;
|
|
case 3: printf("?"); break;
|
|
}
|
|
printf(" data:");
|
|
for (i = 0; i < 18; i++) {
|
|
printf(" %x %x", data[i] & 0xf, data[i] >> 4);
|
|
}
|
|
printf("\n");
|
|
#endif
|
|
|
|
return 120;
|
|
}
|
|
|
|
static int decode_frame(uk0_rx_t *rx, uint64_t *buffer, uint64_t polarity, uint8_t *data, uint32_t *sync, uint8_t *m)
|
|
{
|
|
uint64_t buf0, buf1, buf2, buf3;
|
|
int violation = 0, i, j;
|
|
uint8_t bin1, bin2, in, out = 0;
|
|
|
|
/* correct polarity */
|
|
buf0 = buffer[0] ^= polarity;
|
|
buf1 = buffer[1] ^= polarity;
|
|
buf2 = buffer[2] ^= polarity;
|
|
buf3 = buffer[3] ^= polarity;
|
|
|
|
#ifdef DEBUG_FRAME
|
|
printf("RX frame: ");
|
|
for (int s = 0; s < 120; s++) {
|
|
uint8_t tern = (buffer[s >> 5] >> (2*(s&31))) & 3;
|
|
switch (tern) {
|
|
case 1: printf("+"); break;
|
|
case 2: printf("-"); break;
|
|
case 0: printf("0"); break;
|
|
case 3: printf("?"); break;
|
|
}
|
|
}
|
|
printf("\n");
|
|
#endif
|
|
|
|
/* reorder symbols and convert to binary */
|
|
bin1 = mms43_decode[(buf0 >> 0) & 63];
|
|
if (bin1 == 0xff) violation++;
|
|
bin2 = mms43_decode[(buf0 >> 6) & 63];
|
|
if (bin2 == 0xff) violation++;
|
|
data[0] = bin1 | (bin2 << 4);
|
|
bin1 = mms43_decode[(buf0 >> 12) & 63];
|
|
if (bin1 == 0xff) violation++;
|
|
bin2 = mms43_decode[(buf0 >> 18) & 63];
|
|
if (bin2 == 0xff) violation++;
|
|
data[1] = bin1 | (bin2 << 4);
|
|
bin1 = mms43_decode[(buf0 >> 24) & 63];
|
|
if (bin1 == 0xff) violation++;
|
|
bin2 = mms43_decode[(buf0 >> 30) & 63];
|
|
if (bin2 == 0xff) violation++;
|
|
data[2] = bin1 | (bin2 << 4);
|
|
bin1 = mms43_decode[(buf0 >> 36) & 63];
|
|
if (bin1 == 0xff) violation++;
|
|
bin2 = mms43_decode[(buf0 >> 42) & 63];
|
|
if (bin2 == 0xff) violation++;
|
|
data[3] = bin1 | (bin2 << 4);
|
|
*m = (buf0 >> 48) & 3;
|
|
bin1 = mms43_decode[(buf0 >> 50) & 63];
|
|
if (bin1 == 0xff) violation++;
|
|
bin2 = mms43_decode[(buf0 >> 56) & 63];
|
|
if (bin2 == 0xff) violation++;
|
|
data[4] = bin1 | (bin2 << 4);
|
|
bin1 = mms43_decode[((buf0 >> 62) | (buf1 << 2)) & 63];
|
|
if (bin1 == 0xff) violation++;
|
|
bin2 = mms43_decode[(buf1 >> 4) & 63];
|
|
if (bin2 == 0xff) violation++;
|
|
data[5] = bin1 | (bin2 << 4);
|
|
bin1 = mms43_decode[(buf1 >> 10) & 63];
|
|
if (bin1 == 0xff) violation++;
|
|
bin2 = mms43_decode[(buf1 >> 16) & 63];
|
|
if (bin2 == 0xff) violation++;
|
|
data[6] = bin1 | (bin2 << 4);
|
|
bin1 = mms43_decode[(buf1 >> 22) & 63];
|
|
if (bin1 == 0xff) violation++;
|
|
bin2 = mms43_decode[(buf1 >> 28) & 63];
|
|
if (bin2 == 0xff) violation++;
|
|
data[7] = bin1 | (bin2 << 4);
|
|
*sync = (buf1 >> 34) & SYNC_MASK;
|
|
bin1 = mms43_decode[(buf1 >> 56) & 63];
|
|
if (bin1 == 0xff) violation++;
|
|
bin2 = mms43_decode[((buf1 >> 62) | (buf2 << 2)) & 63];
|
|
if (bin2 == 0xff) violation++;
|
|
data[8] = bin1 | (bin2 << 4);
|
|
bin1 = mms43_decode[(buf2 >> 4) & 63];
|
|
if (bin1 == 0xff) violation++;
|
|
bin2 = mms43_decode[(buf2 >> 10) & 63];
|
|
if (bin2 == 0xff) violation++;
|
|
data[9] = bin1 | (bin2 << 4);
|
|
bin1 = mms43_decode[(buf2 >> 16) & 63];
|
|
if (bin1 == 0xff) violation++;
|
|
bin2 = mms43_decode[(buf2 >> 22) & 63];
|
|
if (bin2 == 0xff) violation++;
|
|
data[10] = bin1 | (bin2 << 4);
|
|
bin1 = mms43_decode[(buf2 >> 28) & 63];
|
|
if (bin1 == 0xff) violation++;
|
|
bin2 = mms43_decode[(buf2 >> 34) & 63];
|
|
if (bin2 == 0xff) violation++;
|
|
data[11] = bin1 | (bin2 << 4);
|
|
bin1 = mms43_decode[(buf2 >> 40) & 63];
|
|
if (bin1 == 0xff) violation++;
|
|
bin2 = mms43_decode[(buf2 >> 46) & 63];
|
|
if (bin2 == 0xff) violation++;
|
|
data[12] = bin1 | (bin2 << 4);
|
|
bin1 = mms43_decode[(buf2 >> 52) & 63];
|
|
if (bin1 == 0xff) violation++;
|
|
bin2 = mms43_decode[(buf2 >> 58) & 63];
|
|
if (bin2 == 0xff) violation++;
|
|
data[13] = bin1 | (bin2 << 4);
|
|
bin1 = mms43_decode[(buf3 >> 0) & 63];
|
|
if (bin1 == 0xff) violation++;
|
|
bin2 = mms43_decode[(buf3 >> 6) & 63];
|
|
if (bin2 == 0xff) violation++;
|
|
data[14] = bin1 | (bin2 << 4);
|
|
bin1 = mms43_decode[(buf3 >> 12) & 63];
|
|
if (bin1 == 0xff) violation++;
|
|
bin2 = mms43_decode[(buf3 >> 18) & 63];
|
|
if (bin2 == 0xff) violation++;
|
|
data[15] = bin1 | (bin2 << 4);
|
|
bin1 = mms43_decode[(buf3 >> 24) & 63];
|
|
if (bin1 == 0xff) violation++;
|
|
bin2 = mms43_decode[(buf3 >> 30) & 63];
|
|
if (bin2 == 0xff) violation++;
|
|
data[16] = bin1 | (bin2 << 4);
|
|
bin1 = mms43_decode[(buf3 >> 36) & 63];
|
|
if (bin1 == 0xff) violation++;
|
|
bin2 = mms43_decode[(buf3 >> 42) & 63];
|
|
if (bin2 == 0xff) violation++;
|
|
data[17] = bin1 | (bin2 << 4);
|
|
|
|
#ifdef DEBUG_FRAME
|
|
printf("sync: ");
|
|
for (int s = 0; s < 11; s++) {
|
|
switch ((*sync) >> (s*2) & 3) {
|
|
case 1: printf("+"); break;
|
|
case 2: printf("-"); break;
|
|
case 0: printf("0"); break;
|
|
case 3: printf("?"); break;
|
|
}
|
|
}
|
|
printf(" M: ");
|
|
switch ((*m)) {
|
|
case 1: printf("+"); break;
|
|
case 2: printf("-"); break;
|
|
case 0: printf("0"); break;
|
|
case 3: printf("?"); break;
|
|
}
|
|
printf(" data:");
|
|
for (i = 0; i < 18; i++) {
|
|
printf(" %x %x", data[i] & 0xf, data[i] >> 4);
|
|
}
|
|
printf(" Violation: %d\n", violation);
|
|
#endif
|
|
|
|
/* descramble */
|
|
#ifdef DEBUG_SCRAMBLER
|
|
printf("RX descram ");
|
|
#endif
|
|
for (i = 0; i < 18; i++) {
|
|
in = data[i];
|
|
for (j = 0; j < 8; j++) {
|
|
out >>= 1;
|
|
out |= ((rx->descramble >> 23) ^ (rx->descramble >> 18) ^ in) << 7;
|
|
rx->descramble |= in & 1;
|
|
rx->descramble <<= 1;
|
|
in >>= 1;
|
|
}
|
|
data[i] = out;
|
|
#ifdef DEBUG_SCRAMBLER
|
|
printf(" %02x", out);
|
|
#endif
|
|
}
|
|
#ifdef DEBUG_SCRAMBLER
|
|
printf("\n");
|
|
#endif
|
|
|
|
return violation;
|
|
}
|
|
|
|
/*
|
|
* modulation and demodulation
|
|
*/
|
|
|
|
static inline uint8_t tx_symbol(uk0_t *uk0, uk0_tx_t *tx)
|
|
{
|
|
uint8_t data[18];
|
|
|
|
if (tx->frame_index == tx->frame_length) {
|
|
#ifdef HDLC_LOOPBACK
|
|
switch (UK0_STATE_LT_1_7) {
|
|
#else
|
|
switch (uk0->state) {
|
|
#endif
|
|
/* in states where no data is sent */
|
|
case UK0_STATE_NULL:
|
|
case UK0_STATE_LT_1_1:
|
|
case UK0_STATE_LT_1_8:
|
|
/* transmit zero level (transmitter off) */
|
|
memset(tx->buffer, 0, sizeof(tx->buffer));
|
|
tx->frame_length = 256;
|
|
break;
|
|
/* sending U2W and waiting for framing (or U1W) */
|
|
case UK0_STATE_LT_1_2:
|
|
case UK0_STATE_LT_1_3:
|
|
if (uk0->timer == uk0->ti1) {
|
|
/* on entering 1.2, send U2W */
|
|
tx->buffer[0] = WAKEUP;
|
|
tx->buffer[1] = WAKEUP;
|
|
tx->buffer[2] = WAKEUP;
|
|
tx->buffer[3] = WAKEUP;
|
|
tx->buffer[4] = WAKEUP;
|
|
tx->buffer[5] = WAKEUP;
|
|
tx->buffer[6] = WAKEUP;
|
|
tx->buffer[7] = WAKEUP;
|
|
tx->frame_length = 256;
|
|
} else if (uk0->timer >= uk0->ti1 - 7) {
|
|
/* then send U0 for 7ms */
|
|
memset(tx->buffer, 0, sizeof(tx->buffer));
|
|
tx->frame_length = 120;
|
|
} else {
|
|
/* afterwards send U2 */
|
|
memset(data, 0x00, 18);
|
|
tx->frame_length = encode_frame(tx, tx->buffer, data, SYNC_LT_POSITIVE, 0);
|
|
}
|
|
if (--uk0->timer == 0) {
|
|
uk0_handle_event(uk0, UK0_EVENT_TIMEOUT);
|
|
}
|
|
break;
|
|
case UK0_STATE_LT_1_4:
|
|
case UK0_STATE_LT_1_5:
|
|
/* send U2 */
|
|
memset(data, 0x00, 18);
|
|
tx->frame_length = encode_frame(tx, tx->buffer, data, SYNC_LT_POSITIVE, 0);
|
|
break;
|
|
case UK0_STATE_LT_1_6:
|
|
/* send U4H */
|
|
memset(data, 0xff, 18);
|
|
tx->frame_length = encode_frame(tx, tx->buffer, data, SYNC_LT_POSITIVE, 0);
|
|
if (--uk0->timer == 0) {
|
|
uk0_handle_event(uk0, UK0_EVENT_TIMEOUT);
|
|
}
|
|
break;
|
|
/* send U4 */
|
|
case UK0_STATE_LT_1_7:
|
|
mux_frame(uk0, data);
|
|
/* send all 0xff on loopback test, so we can detect if it is not received that way */
|
|
if (uk0->loopback2)
|
|
memset(data, 0xff, sizeof(data));
|
|
tx->frame_length = encode_frame(tx, tx->buffer, data, SYNC_LT_POSITIVE, uk0->loopback2);
|
|
break;
|
|
}
|
|
tx->frame_index = 0;
|
|
}
|
|
//printf("%c\n", tx->frame_data[tx->frame_index]);
|
|
return (tx->buffer[tx->frame_index >> 5] >> ((tx->frame_index & 31) * 2)) & 3;
|
|
}
|
|
|
|
void uk0_encode_mms43(uk0_t *uk0, sample_t *samples, int length)
|
|
{
|
|
uk0_tx_t *tx = &uk0->tx;
|
|
int i;
|
|
uint8_t symbol;
|
|
|
|
/* for all samples */
|
|
for (i = 0; i < length; i++) {
|
|
tx->phase += tx->phase_step;
|
|
if (tx->phase >= 1.0) {
|
|
/* symbol has changed */
|
|
tx->phase -= 1.0;
|
|
tx->index++;
|
|
tx->last_level = tx->level;
|
|
symbol = tx_symbol(uk0, tx);
|
|
if (symbol == 1)
|
|
tx->level = 1.0;
|
|
else if (symbol == 2)
|
|
tx->level = -1.0;
|
|
else
|
|
tx->level = 0.0;
|
|
tx->frame_index++;
|
|
}
|
|
*samples++ = ((cos(tx->phase * M_PI) / 2.0 - 0.5) * (tx->last_level - tx->level) + tx->last_level) * TX_LEVEL;
|
|
}
|
|
}
|
|
|
|
//#define DEBUG_RX_FRAME
|
|
|
|
static inline void rx_symbol(uk0_t *uk0, uk0_rx_t *rx, double level_error)
|
|
{
|
|
uint8_t data[18];
|
|
uint32_t sync;
|
|
uint8_t m;
|
|
int i, not0, not1;
|
|
|
|
/* process level measurement */
|
|
if (uk0->measure_level_interval) {
|
|
uk0->measure_level_error += level_error;
|
|
if (++uk0->measure_level_index == uk0->measure_level_interval) {
|
|
level_error = uk0->measure_level_error / uk0->measure_level_index;
|
|
uk0->measure_level_index = 0;
|
|
uk0->measure_level_error = 0.0;
|
|
display_measurements_update(uk0->dmp_level, rx->rx_level * 100.0, 0.0);
|
|
if (uk0->state >= UK0_STATE_LT_1_4)
|
|
display_measurements_update(uk0->dmp_level_error, level_error * 100.0, 0.0);
|
|
}
|
|
}
|
|
|
|
/* lost framing */
|
|
if (rx->buffer[0] == 0 && rx->u0_detect) {
|
|
if (--rx->u0_detect == 0) {
|
|
#ifdef DEBUG_RX_FRAME
|
|
puts("lost frame");
|
|
#endif
|
|
/* only send if we are not already in idle state */
|
|
if (uk0->state != UK0_STATE_LT_1_1) {
|
|
uk0_handle_event(uk0, UK0_EVENT_U0);
|
|
}
|
|
return;
|
|
}
|
|
}
|
|
|
|
/* detect wakeup, but wait 256 symbols to cease */
|
|
if (rx->u1w_count) {
|
|
rx->u1w_count--;
|
|
} else {
|
|
if (rx->buffer[0] == WAKEUP && rx->buffer[1] == WAKEUP) {
|
|
rx->u1w_count = 256;
|
|
#ifdef DEBUG_RX_FRAME
|
|
puts("wakeup signal");
|
|
#endif
|
|
uk0_handle_event(uk0, UK0_EVENT_U1W);
|
|
return;
|
|
}
|
|
}
|
|
|
|
/* various sync detection */
|
|
sync = (rx->buffer[1] >> 34) & SYNC_MASK;
|
|
if (rx->u1a_count) {
|
|
rx->u1a_count--;
|
|
if (rx->u1a_count == 0) {
|
|
if (sync == 0 && rx->buffer[0] != 0 && rx->buffer[1] != 0 && rx->buffer[2] != 0 && rx->buffer[3] != 0) {
|
|
rx->u1a_count = 120;
|
|
if (rx->u1a_detect < 4) {
|
|
rx->u1a_detect++;
|
|
} else {
|
|
rx->u0_detect = LOS_CRITERION;
|
|
#ifdef DEBUG_RX_FRAME
|
|
puts("frame sync=0");
|
|
#endif
|
|
uk0_handle_event(uk0, UK0_EVENT_U1A);
|
|
return;
|
|
}
|
|
} else {
|
|
rx->u1a_detect = 0;
|
|
}
|
|
}
|
|
} else {
|
|
if (sync == 0 && rx->buffer[0] != 0 && rx->buffer[1] != 0 && rx->buffer[2] != 0 && rx->buffer[3] != 0) {
|
|
rx->u1a_count = 120;
|
|
}
|
|
}
|
|
|
|
if (rx->sync_count) {
|
|
/* when 'in sync', we count until we expect next sync sequence */
|
|
if (--rx->sync_symbols > 0)
|
|
return;
|
|
/* when we actually got a sync sequence, we reset sync counter, else we let it count to 0 */
|
|
rx->sync_count--;
|
|
rx->sync_symbols = 120;
|
|
if (sync == rx->sync_last)
|
|
rx->sync_count = SYNC_RECOVER;
|
|
rx->u0_detect = LOS_CRITERION;
|
|
if (rx->sync_count == 0)
|
|
PDEBUG_CHAN(DUK0, DEBUG_INFO, "Lost sync\n");
|
|
if (rx->sync_last == SYNC_NT_POSITIVE)
|
|
decode_frame(rx, rx->buffer, 0x0000000000000000, data, &sync, &m);
|
|
else if (rx->sync_last == SYNC_NT_NEGATIVE)
|
|
decode_frame(rx, rx->buffer, 0xffffffffffffffff, data, &sync, &m);
|
|
else
|
|
return;
|
|
} else {
|
|
/* when we are not in sync, we take whatever sync sequence we receive and become 'in sync' */
|
|
if (sync == SYNC_NT_POSITIVE) {
|
|
PDEBUG_CHAN(DUK0, DEBUG_INFO, "Sync detected\n");
|
|
rx->sync_last = sync;
|
|
rx->sync_count = SYNC_RECOVER;
|
|
rx->sync_symbols = 120;
|
|
rx->u0_detect = LOS_CRITERION;
|
|
decode_frame(rx, rx->buffer, 0x0000000000000000, data, &sync, &m);
|
|
} else if (sync == SYNC_NT_NEGATIVE) {
|
|
PDEBUG_CHAN(DUK0, DEBUG_DEBUG, "Sync detected\n");
|
|
rx->sync_last = sync;
|
|
rx->sync_count = SYNC_RECOVER;
|
|
rx->sync_symbols = 120;
|
|
rx->u0_detect = LOS_CRITERION;
|
|
decode_frame(rx, rx->buffer, 0xffffffffffffffff, data, &sync, &m);
|
|
} else
|
|
return;
|
|
}
|
|
|
|
/* loopback frame error reporting */
|
|
if (uk0->state == UK0_STATE_LT_1_7 && uk0->loopback2) {
|
|
not1 = 0;
|
|
for (i = 0; i < 18; i++) {
|
|
if (data[i] != 0xff)
|
|
not1 = 1;
|
|
}
|
|
if (not1) {
|
|
uk0->measure_frame_error++;
|
|
#ifdef DEBUG_RX_FRAME
|
|
puts("frame error in loopback mode");
|
|
#endif
|
|
}
|
|
if (++uk0->measure_frame_index == uk0->measure_frame_interval) {
|
|
if (uk0->measure_frame_error)
|
|
PDEBUG_CHAN(DUK0, DEBUG_NOTICE, "Loopback 2 test: %d frame error%s received!\n", uk0->measure_frame_error, (uk0->measure_frame_error != 1) ? "s" : "");
|
|
uk0->measure_frame_index = 0;
|
|
uk0->measure_frame_error = 0;
|
|
}
|
|
} else
|
|
uk0->measure_frame_error = 0;
|
|
|
|
/* receive valid frame */
|
|
if (uk0->state == UK0_STATE_LT_1_7) {
|
|
demux_frame(uk0, data);
|
|
return;
|
|
}
|
|
|
|
/* U1 and U3 detection */
|
|
not0 = not1 = 0;
|
|
for (i = 0; i < 18; i++) {
|
|
if (data[i] != 0x00)
|
|
not0 = 1;
|
|
if (data[i] != 0xff)
|
|
not1 = 1;
|
|
}
|
|
/* all data is 0 */
|
|
if (!not0) {
|
|
#ifdef DEBUG_RX_FRAME
|
|
puts("frame sync=1 data=0");
|
|
#endif
|
|
uk0_handle_event(uk0, UK0_EVENT_U1);
|
|
return;
|
|
}
|
|
/* all data is 1 */
|
|
if (!not1) {
|
|
#ifdef DEBUG_RX_FRAME
|
|
puts("frame sync=1 data=1");
|
|
#endif
|
|
uk0_handle_event(uk0, UK0_EVENT_U3);
|
|
return;
|
|
}
|
|
}
|
|
|
|
void uk0_decode_mms43(uk0_t *uk0, sample_t *samples, int length)
|
|
{
|
|
uk0_rx_t *rx = &uk0->rx;
|
|
int i, ii, ll = length * RX_OVERSAMPLING, j;
|
|
sample_t upsample[ll], I[ll], Q[ll];
|
|
double spl;
|
|
double phase;
|
|
double slope;
|
|
double offset, sample_phase, error;
|
|
char __attribute__((unused)) symbol;
|
|
double level_error;
|
|
|
|
display_wave(&uk0->dispwav, samples, length, 1.0);
|
|
|
|
/* upsample received signal, using SINC */
|
|
for (i = 0, ii = 0; i < length; i++) {
|
|
/* get upsampled portion from window */
|
|
for (j = 0; j < RX_OVERSAMPLING; j++) {
|
|
upsample[ii++] = rx->window[j];
|
|
// puts(debug_amplitude(rx->window[j]));
|
|
}
|
|
/* shift window */
|
|
for (j = 0; j < rx->window_length - RX_OVERSAMPLING; j++)
|
|
rx->window[j] = rx->window[j + RX_OVERSAMPLING];
|
|
/* insert zeroes at the end of window */
|
|
for (; j < rx->window_length; j++)
|
|
rx->window[j] = 0.0;
|
|
/* add sample as SINC curve to window */
|
|
spl = samples[i];
|
|
for (j = 0; j < rx->window_length; j++)
|
|
rx->window[j] += rx->sinc[j] * spl;
|
|
}
|
|
|
|
/* recover clock from symbol change, using slope of signal */
|
|
phase = rx->phase;
|
|
for (ii = 0; ii < ll; ii++) {
|
|
/* slope of signal */
|
|
slope = fabs(upsample[ii] - rx->last_level);
|
|
// printf("%03d %.3f %s\n", (int)(rx->phase / M_PI * 180.0), slope, debug_amplitude(slope * 10));
|
|
rx->last_level = upsample[ii];
|
|
/* modulate with bit clock */
|
|
I[ii] = cos(phase) * slope;
|
|
Q[ii] = sin(phase) * slope;
|
|
phase += rx->phase_step;
|
|
if (phase >= 2.0 * M_PI)
|
|
phase -= 2.0 * M_PI;
|
|
}
|
|
/* filter clock */
|
|
iir_process(&rx->lp[0], I, ll);
|
|
iir_process(&rx->lp[1], Q, ll);
|
|
|
|
for (ii = 0; ii < ll; ii++) {
|
|
/* calculate min and max */
|
|
if (upsample[ii] < rx->min)
|
|
rx->min = upsample[ii];
|
|
if (upsample[ii] > rx->max)
|
|
rx->max = upsample[ii];
|
|
/* every ms restart min/max calculation and correct sample phase */
|
|
rx->count_ms += rx->ms_per_sample;
|
|
if (rx->count_ms >= 1.0) {
|
|
rx->count_ms -= 1.0;
|
|
/* calculate level thresholds */
|
|
rx->zero = (rx->max + rx->min) / 2.0;
|
|
if ((rx->max - rx->min) / 2.0 > MIN_LEVEL) {
|
|
rx->high = rx->max;
|
|
rx->low = rx->min;
|
|
rx->high_threshold = (rx->max + rx->zero) / 2.0;
|
|
rx->low_threshold = (rx->min + rx->zero) / 2.0;
|
|
} else {
|
|
rx->high = rx->zero + MIN_LEVEL;
|
|
rx->low = rx->zero - MIN_LEVEL;
|
|
rx->high_threshold = rx->zero + MIN_LEVEL / 2.0;
|
|
rx->low_threshold = rx->zero - MIN_LEVEL / 2.0;
|
|
}
|
|
rx->rx_level = (rx->max - rx->min) / 2.0;
|
|
// printf("min=%.2f max=%.2f ", rx->min, rx->max);
|
|
#if 0
|
|
if ((rx->max - rx->min) > MIN_LEVEL)
|
|
rx->has_signal = 1;
|
|
else
|
|
rx->has_signal = 0;
|
|
#endif
|
|
/* reset min/max */
|
|
rx->min = rx->max = upsample[ii];
|
|
/* calculate phase offset */
|
|
offset = atan2(Q[0], I[0]);
|
|
// printf("offset=%03d ", (int)(offset / M_PI * 180.0));
|
|
/* calculate sample phase */
|
|
sample_phase = fmod(rx->phase + 3.0 * M_PI - offset, 2.0 * M_PI);
|
|
/* calculate error */
|
|
error = fmod(sample_phase + 2.0 * M_PI - rx->sample_phase, 2.0 * M_PI);
|
|
if (error > M_PI)
|
|
error -= 2.0 * M_PI;
|
|
// printf("error=%.8f\n", error);
|
|
/* correct using error */
|
|
rx->sample_phase += error;
|
|
}
|
|
/* discriminate symbols from signal amplitude using clock and level range */
|
|
// printf("%03d %.3f %s\n", (int)(rx->sample_phase / M_PI * 180.0), upsample[ii], debug_amplitude(upsample[ii]));
|
|
rx->sample_phase += rx->phase_step;
|
|
if (rx->sample_phase >= 2.0 * M_PI) {
|
|
rx->sample_phase -= 2.0 * M_PI;
|
|
// puts("ping!");
|
|
/* shift rx buffer */
|
|
rx->buffer[0] >>= 2;
|
|
rx->buffer[0] |= rx->buffer[1] << 62;
|
|
rx->buffer[1] >>= 2;
|
|
rx->buffer[1] |= rx->buffer[2] << 62;
|
|
rx->buffer[2] >>= 2;
|
|
rx->buffer[2] |= rx->buffer[3] << 62;
|
|
rx->buffer[3] >>= 2;
|
|
if (upsample[ii] > rx->high_threshold) {
|
|
symbol = '+';
|
|
rx->buffer[3] |= 1LL << 46;
|
|
if (uk0->measure_level_interval)
|
|
level_error = fabs((upsample[ii] - rx->high) / (rx->rx_level / 2.0));
|
|
} else if (upsample[ii] < rx->low_threshold) {
|
|
symbol = '-';
|
|
rx->buffer[3] |= 2LL << 46;
|
|
if (uk0->measure_level_interval)
|
|
level_error = fabs((upsample[ii] - rx->low) / (rx->rx_level / 2.0));
|
|
} else {
|
|
symbol = '0';
|
|
if (uk0->measure_level_interval)
|
|
level_error = fabs((upsample[ii] - rx->zero) / (rx->rx_level / 2.0));
|
|
}
|
|
// printf("%.2f ", level_error);
|
|
// printf("%c", symbol);
|
|
rx_symbol(uk0, rx, level_error);
|
|
}
|
|
/* forward clock phase */
|
|
rx->phase += rx->phase_step;
|
|
}
|
|
|
|
#ifdef DEBUG_RECORD_RX
|
|
#ifdef DEBUG_RECORD_UPSAMPLE
|
|
sample_t *record[1] = { upsample };
|
|
wave_write(&rx->rec, record, ll);
|
|
#else
|
|
sample_t *record[1] = { samples };
|
|
wave_write(&rx->rec, record, length);
|
|
#endif
|
|
#endif
|
|
}
|
|
|