mirror of
https://github.com/portapack-mayhem/mayhem-firmware.git
synced 2026-08-08 17:03:49 +00:00
VasylSamoilov
fc5beb0c09
- Fix long-address numeric message decoding: read body[0] from Vy (j+1) for multi-word messages, from w1 for single-word. Fix MF word count for long addresses (n_field words, not n_field+1). - Fix long-address short message: read 5 additional BCD digits from Vy for 8-digit decode. - Fix BCD table: index 10 is '.' (dot) not ' ' (space) in both RX and TX. - Clean up address decode: identify address type by value range per Table 3.8.1-1. Remove is_group/is_temp_group from packet structs. - Rename SMSG to SHORT. Format message payload directly in baseband: TONE, NUM abc, SRC N, SRC N N=M R=R, RESERVED hex. - Add BIW1 heartbeat packet (biw_field=0xFF) on every decoded frame. - Move timezone table to file scope, simplify console log_message, add No signal initial status, simplify serial BIW output format.
1495 lines
51 KiB
C++
1495 lines
51 KiB
C++
#include "proc_flex.hpp"
|
|
#include "event_m4.hpp"
|
|
#include "audio_dma.hpp"
|
|
#include "pocsag.hpp"
|
|
#include "dsp_fir_taps.hpp"
|
|
#include "portapack_shared_memory.hpp"
|
|
|
|
#include <cmath>
|
|
#include <cstring>
|
|
|
|
// BCD character table for FLEX numeric messages (index 0-15)
|
|
static const char flex_bcd[] = "0123456789.U -][";
|
|
|
|
// Lightweight string helpers (no snprintf/heap on bare-metal M4)
|
|
namespace {
|
|
|
|
char* str_append(char* dst, const char* end, const char* src) {
|
|
while (*src && dst < end - 1) *dst++ = *src++;
|
|
*dst = '\0';
|
|
return dst;
|
|
}
|
|
|
|
char* str_uint(char* dst, const char* end, uint32_t val, int min_digits = 1) {
|
|
char tmp[11];
|
|
int i = 0;
|
|
if (val == 0) {
|
|
tmp[i++] = '0';
|
|
} else {
|
|
while (val > 0) {
|
|
tmp[i++] = '0' + (val % 10);
|
|
val /= 10;
|
|
}
|
|
}
|
|
while (i < min_digits) tmp[i++] = '0';
|
|
for (int j = i - 1; j >= 0 && dst < end - 1; j--) *dst++ = tmp[j];
|
|
*dst = '\0';
|
|
return dst;
|
|
}
|
|
|
|
char* str_hex(char* dst, const char* end, uint32_t val, int digits) {
|
|
static const char hex[] = "0123456789ABCDEF";
|
|
for (int i = digits - 1; i >= 0 && dst < end - 1; i--)
|
|
*dst++ = hex[(val >> (i * 4)) & 0xF];
|
|
*dst = '\0';
|
|
return dst;
|
|
}
|
|
|
|
} // namespace
|
|
|
|
// Constants from demod_flex.c
|
|
#define FREQ_SAMP 24000 // Our sample rate
|
|
#define DC_OFFSET_FILTER 0.010
|
|
#define PHASE_LOCKED_RATE 0.045
|
|
#define PHASE_UNLOCKED_RATE 0.050
|
|
#define LOCK_LEN 24
|
|
#define IDLE_THRESHOLD 0
|
|
#define DEMOD_TIMEOUT 100
|
|
#define FLEX_SYNC_MARKER 0xA6C6AAAAul
|
|
#define SLICE_THRESHOLD 0.667
|
|
|
|
// Implement EccContainer here to avoid linking pocsag.cpp which pulls in app headers
|
|
using namespace pocsag;
|
|
|
|
EccContainer::EccContainer() {
|
|
setup_ecc();
|
|
}
|
|
|
|
void EccContainer::setup_ecc() {
|
|
unsigned int srr = 0x3b4;
|
|
unsigned int i, n, j, k;
|
|
|
|
for (i = 0; i <= 20; i++) {
|
|
ecs[i] = srr;
|
|
if ((srr & 0x01) != 0)
|
|
srr = (srr >> 1) ^ 0x3B4;
|
|
else
|
|
srr = srr >> 1;
|
|
}
|
|
|
|
for (i = 0; i < 1024; i++) bch[i] = 0;
|
|
|
|
for (n = 0; n <= 20; n++) {
|
|
for (i = 0; i <= 20; i++) {
|
|
j = (i << 5) + n;
|
|
k = ecs[n] ^ ecs[i];
|
|
bch[k] = j + 0x2000;
|
|
}
|
|
}
|
|
|
|
for (n = 0; n <= 20; n++) {
|
|
k = ecs[n];
|
|
j = n + (0x1f << 5);
|
|
bch[k] = j + 0x1000;
|
|
}
|
|
|
|
for (n = 0; n <= 20; n++) {
|
|
for (i = 0; i < 10; i++) {
|
|
k = ecs[n] ^ (1 << i);
|
|
j = n + (0x1f << 5);
|
|
bch[k] = j + 0x2000;
|
|
}
|
|
}
|
|
|
|
for (n = 0; n < 10; n++) {
|
|
k = 1 << n;
|
|
bch[k] = 0x3ff + 0x1000;
|
|
}
|
|
|
|
for (n = 0; n < 10; n++) {
|
|
for (i = 0; i < 10; i++) {
|
|
if (i != n) {
|
|
k = (1 << n) ^ (1 << i);
|
|
bch[k] = 0x3ff + 0x2000;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
int EccContainer::error_correct(uint32_t& val) {
|
|
int i, synd, errl, acc, pari, ecc, b1, b2;
|
|
|
|
errl = 0;
|
|
pari = 0;
|
|
|
|
ecc = 0;
|
|
for (i = 31; i >= 11; --i) {
|
|
if (val & (1 << i)) {
|
|
ecc = ecc ^ ecs[31 - i];
|
|
pari = pari ^ 0x01;
|
|
}
|
|
}
|
|
|
|
acc = 0;
|
|
for (i = 10; i >= 1; --i) {
|
|
acc = acc << 1;
|
|
if (val & (1 << i)) {
|
|
acc = acc ^ 0x01;
|
|
}
|
|
}
|
|
|
|
synd = ecc ^ acc;
|
|
errl = 0;
|
|
|
|
if (synd != 0) {
|
|
if (bch[synd] != 0) {
|
|
b1 = bch[synd] & 0x1f;
|
|
b2 = bch[synd] >> 5;
|
|
b2 = b2 & 0x1f;
|
|
|
|
if (b2 != 0x1f) {
|
|
val ^= 0x01 << (31 - b2);
|
|
ecc = ecc ^ ecs[b2];
|
|
}
|
|
|
|
if (b1 != 0x1f) {
|
|
val ^= 0x01 << (31 - b1);
|
|
ecc = ecc ^ ecs[b1];
|
|
}
|
|
|
|
errl = bch[synd] >> 12;
|
|
} else {
|
|
errl = 3;
|
|
}
|
|
|
|
if (errl == 1) pari = pari ^ 0x01;
|
|
}
|
|
|
|
if (errl == 4) errl = 3;
|
|
|
|
return errl;
|
|
}
|
|
|
|
namespace {
|
|
|
|
// Helpers
|
|
unsigned int popcount(unsigned int n) {
|
|
// Simple popcount for 32-bit integer
|
|
n = n - ((n >> 1) & 0x55555555);
|
|
n = (n & 0x33333333) + ((n >> 2) & 0x33333333);
|
|
return (((n + (n >> 4)) & 0x0F0F0F0F) * 0x01010101) >> 24;
|
|
}
|
|
|
|
uint32_t bit_reverse_32(uint32_t x) {
|
|
x = ((x >> 1) & 0x55555555) | ((x & 0x55555555) << 1);
|
|
x = ((x >> 2) & 0x33333333) | ((x & 0x33333333) << 2);
|
|
x = ((x >> 4) & 0x0F0F0F0F) | ((x & 0x0F0F0F0F) << 4);
|
|
x = ((x >> 8) & 0x00FF00FF) | ((x & 0x00FF00FF) << 8);
|
|
x = (x >> 16) | (x << 16);
|
|
return x;
|
|
}
|
|
|
|
} // namespace
|
|
|
|
void FlexProcessor::send_debug(const char* text, uint32_t v1, uint32_t v2) {
|
|
if (shared_memory.application_queue.is_empty()) return;
|
|
FlexDebugMessage message(v1, v2, text);
|
|
shared_memory.application_queue.push(message);
|
|
}
|
|
|
|
void FlexProcessor::execute(const buffer_c8_t& buffer) {
|
|
if (!configured) return;
|
|
|
|
// Heartbeat debug every ~1 second (24000Hz / 4096 buffer size * ~6)
|
|
static int debug_count = 0;
|
|
debug_count++;
|
|
if (debug_count > 1000) {
|
|
send_debug("Running", 0, 0);
|
|
debug_count = 0;
|
|
}
|
|
|
|
// Decimate and demodulate: 3.072MHz -> 24kHz
|
|
auto decim_0_out = decim_0_iq.execute(buffer, dst_buffer);
|
|
auto decim_1_out = decim_1_iq.execute(decim_0_out, dst_buffer);
|
|
auto channel_out = channel_filter.execute(decim_1_out, dst_buffer);
|
|
auto audio = demod.execute(channel_out, audio_buffer);
|
|
|
|
process_audio(audio);
|
|
}
|
|
|
|
void FlexProcessor::process_audio(const buffer_f32_t& audio) {
|
|
for (size_t i = 0; i < audio.count; ++i) {
|
|
flex_demodulate(audio.p[i]);
|
|
}
|
|
}
|
|
|
|
void FlexProcessor::flex_demodulate(double sample) {
|
|
if (build_symbol(sample) == 1) {
|
|
demodulator.nonconsec = 0;
|
|
demodulator.symbol_count++;
|
|
// modulation.symbol_rate = ... // Unused in main logic usually, just stats
|
|
|
|
/*Determine the modal symbol*/
|
|
int j;
|
|
int decmax = 0;
|
|
int modal_symbol = 0;
|
|
for (j = 0; j < 4; j++) {
|
|
if (demodulator.symcount[j] > decmax) {
|
|
modal_symbol = j;
|
|
decmax = demodulator.symcount[j];
|
|
}
|
|
}
|
|
demodulator.symcount[0] = 0;
|
|
demodulator.symcount[1] = 0;
|
|
demodulator.symcount[2] = 0;
|
|
demodulator.symcount[3] = 0;
|
|
|
|
if (demodulator.locked) {
|
|
/*Process the symbol*/
|
|
flex_sym(modal_symbol);
|
|
} else {
|
|
/*Check for lock pattern*/
|
|
/*Shift symbols into buffer, symbols are converted so that the max and min symbols map to 1 and 2 i.e each contain a single 1 */
|
|
demodulator.lock_buf = (demodulator.lock_buf << 2) | (modal_symbol ^ 0x1);
|
|
uint64_t lock_pattern = demodulator.lock_buf ^ 0x6666666666666666ull;
|
|
uint64_t lock_mask = (1ull << (2 * LOCK_LEN)) - 1;
|
|
|
|
if ((lock_pattern & lock_mask) == 0 || ((~lock_pattern) & lock_mask) == 0) {
|
|
demodulator.locked = 1;
|
|
demodulator.lock_buf = 0;
|
|
demodulator.symbol_count = 0;
|
|
demodulator.sample_count = 0;
|
|
}
|
|
}
|
|
|
|
/*Time out after X periods with no zero crossing*/
|
|
demodulator.timeout++;
|
|
if (demodulator.timeout > DEMOD_TIMEOUT) {
|
|
demodulator.locked = 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
int FlexProcessor::build_symbol(double sample) {
|
|
const int64_t phase_max = 100 * demodulator.sample_freq;
|
|
const int64_t phase_rate = phase_max * demodulator.baud / demodulator.sample_freq;
|
|
const double phasepercent = 100.0 * demodulator.phase / phase_max;
|
|
|
|
demodulator.sample_count++;
|
|
|
|
/*Remove DC offset (FIR filter)*/
|
|
if (state.Current == flex::State::SYNC1) {
|
|
modulation.zero = (modulation.zero * (FREQ_SAMP * DC_OFFSET_FILTER) + sample) / ((FREQ_SAMP * DC_OFFSET_FILTER) + 1);
|
|
}
|
|
sample -= modulation.zero;
|
|
|
|
if (demodulator.locked) {
|
|
if (state.Current == flex::State::SYNC1) {
|
|
demodulator.envelope_sum += std::abs(sample);
|
|
demodulator.envelope_count++;
|
|
modulation.envelope = demodulator.envelope_sum / demodulator.envelope_count;
|
|
}
|
|
} else {
|
|
modulation.envelope = 0;
|
|
demodulator.envelope_sum = 0;
|
|
demodulator.envelope_count = 0;
|
|
demodulator.baud = 1600;
|
|
demodulator.timeout = 0;
|
|
demodulator.nonconsec = 0;
|
|
state.Current = flex::State::SYNC1;
|
|
}
|
|
|
|
/* MID 80% SYMBOL PERIOD */
|
|
if (phasepercent > 10 && phasepercent < 90) {
|
|
if (sample > 0) {
|
|
if (sample > modulation.envelope * SLICE_THRESHOLD)
|
|
demodulator.symcount[3]++;
|
|
else
|
|
demodulator.symcount[2]++;
|
|
} else {
|
|
if (sample < -modulation.envelope * SLICE_THRESHOLD)
|
|
demodulator.symcount[0]++;
|
|
else
|
|
demodulator.symcount[1]++;
|
|
}
|
|
}
|
|
|
|
/* ZERO CROSSING */
|
|
if ((demodulator.sample_last < 0 && sample >= 0) || (demodulator.sample_last >= 0 && sample < 0)) {
|
|
double phase_error = 0.0;
|
|
if (phasepercent < 50) {
|
|
phase_error = demodulator.phase;
|
|
} else {
|
|
phase_error = demodulator.phase - phase_max;
|
|
}
|
|
|
|
if (demodulator.locked) {
|
|
demodulator.phase -= phase_error * PHASE_LOCKED_RATE;
|
|
} else {
|
|
demodulator.phase -= phase_error * PHASE_UNLOCKED_RATE;
|
|
}
|
|
|
|
if (phasepercent > 10 && phasepercent < 90) {
|
|
demodulator.nonconsec++;
|
|
if (demodulator.nonconsec > 20 && demodulator.locked) {
|
|
demodulator.locked = 0;
|
|
}
|
|
} else {
|
|
demodulator.nonconsec = 0;
|
|
}
|
|
|
|
demodulator.timeout = 0;
|
|
}
|
|
demodulator.sample_last = sample;
|
|
|
|
/* END OF SYMBOL PERIOD */
|
|
demodulator.phase += phase_rate;
|
|
|
|
if (demodulator.phase > phase_max) {
|
|
demodulator.phase -= phase_max;
|
|
return 1;
|
|
} else {
|
|
return 0;
|
|
}
|
|
}
|
|
|
|
unsigned int FlexProcessor::flex_sync(unsigned char sym) {
|
|
int retval = 0;
|
|
sync.syncbuf = (sync.syncbuf << 1) | ((sym < 2) ? 1 : 0);
|
|
|
|
retval = flex_sync_check(sync.syncbuf);
|
|
if (retval != 0) {
|
|
sync.polarity = 0;
|
|
} else {
|
|
retval = flex_sync_check(~sync.syncbuf);
|
|
if (retval != 0) {
|
|
sync.polarity = 1;
|
|
}
|
|
}
|
|
return retval;
|
|
}
|
|
|
|
unsigned int FlexProcessor::flex_sync_check(uint64_t buf) {
|
|
// 64-bit FLEX sync code: AAAA:BBBBBBBB:CCCC
|
|
unsigned int marker = (buf & 0x0000FFFFFFFF0000ULL) >> 16;
|
|
unsigned short codehigh = (buf & 0xFFFF000000000000ULL) >> 48;
|
|
unsigned short codelow = ~(buf & 0x000000000000FFFFULL);
|
|
|
|
int retval = 0;
|
|
// Hamming distance check (popcount of XOR)
|
|
unsigned int diff_marker = popcount(marker ^ FLEX_SYNC_MARKER);
|
|
unsigned int diff_code = popcount(codelow ^ codehigh);
|
|
|
|
if (diff_marker < 4 && diff_code < 4) {
|
|
retval = codehigh;
|
|
} else {
|
|
retval = 0;
|
|
}
|
|
return retval;
|
|
}
|
|
|
|
void FlexProcessor::decode_mode(unsigned int sync_code) {
|
|
struct FlexModeDef {
|
|
int sync;
|
|
unsigned int baud;
|
|
unsigned int levels;
|
|
} flex_modes[] = {
|
|
{0x870C, 1600, 2},
|
|
{0xB068, 1600, 4},
|
|
{0x7B18, 3200, 2},
|
|
{0xDEA0, 3200, 4},
|
|
{0x4C7C, 3200, 4},
|
|
{0, 0, 0}};
|
|
|
|
for (int i = 0; flex_modes[i].sync != 0; i++) {
|
|
unsigned int diff = popcount((unsigned int)flex_modes[i].sync ^ sync_code);
|
|
if (diff < 4) {
|
|
sync.sync = sync_code;
|
|
sync.baud = flex_modes[i].baud;
|
|
sync.levels = flex_modes[i].levels;
|
|
return;
|
|
}
|
|
}
|
|
// Default
|
|
sync.baud = 1600;
|
|
sync.levels = 2;
|
|
}
|
|
|
|
void FlexProcessor::read_2fsk(unsigned int sym, uint32_t* dat) {
|
|
*dat = (*dat >> 1) | ((sym > 1) ? 0x80000000 : 0);
|
|
}
|
|
|
|
int FlexProcessor::bch_fix_errors(uint32_t* data_to_fix) {
|
|
// Reverse bits for EccContainer (POCSAG MSB-first expectation vs FLEX LSB-first in our representation)
|
|
uint32_t reversed = bit_reverse_32(*data_to_fix);
|
|
int result = ecc.error_correct(reversed);
|
|
if (result == 0 || result == 1 || result == 2) {
|
|
*data_to_fix = bit_reverse_32(reversed);
|
|
}
|
|
return result;
|
|
}
|
|
|
|
int FlexProcessor::decode_fiw() {
|
|
uint32_t fiw_val = fiw.rawdata;
|
|
int decode_error = bch_fix_errors(&fiw_val);
|
|
|
|
if (decode_error > 2) {
|
|
return 1;
|
|
}
|
|
|
|
fiw.checksum = fiw_val & 0xF;
|
|
fiw.cycleno = (fiw_val >> 4) & 0xF;
|
|
fiw.frameno = (fiw_val >> 8) & 0x7F;
|
|
fiw.roaming = (fiw_val >> 15) & 0x01;
|
|
fiw.repeat = (fiw_val >> 16) & 0x01;
|
|
fiw.traffic = (fiw_val >> 17) & 0x0F;
|
|
|
|
unsigned int checksum = (fiw_val & 0xF);
|
|
checksum += ((fiw_val >> 4) & 0xF);
|
|
checksum += ((fiw_val >> 8) & 0xF);
|
|
checksum += ((fiw_val >> 12) & 0xF);
|
|
checksum += ((fiw_val >> 16) & 0xF);
|
|
checksum += ((fiw_val >> 20) & 0x01);
|
|
checksum &= 0xF;
|
|
|
|
if (checksum == 0xF) {
|
|
return 0;
|
|
} else {
|
|
return 1;
|
|
}
|
|
}
|
|
|
|
int FlexProcessor::read_data(unsigned char sym) {
|
|
int bit_a = (sym > 1);
|
|
int bit_b = 0;
|
|
if (sync.levels == 4) {
|
|
bit_b = (sym == 1) || (sym == 2);
|
|
}
|
|
|
|
if (sync.baud == 1600) {
|
|
data.phase_toggle = 0;
|
|
}
|
|
|
|
unsigned int idx = ((data.data_bit_counter >> 5) & 0xFFF8) | (data.data_bit_counter & 0x0007);
|
|
if (idx >= 88) return 0; // Boundary check
|
|
|
|
if (data.phase_toggle == 0) {
|
|
data.PhaseA.buf[idx] = (data.PhaseA.buf[idx] >> 1) | (bit_a ? 0x80000000 : 0);
|
|
data.PhaseB.buf[idx] = (data.PhaseB.buf[idx] >> 1) | (bit_b ? 0x80000000 : 0);
|
|
data.phase_toggle = 1;
|
|
|
|
if ((data.data_bit_counter & 0xFF) == 0xFF) {
|
|
if (data.PhaseA.buf[idx] == 0x00000000 || data.PhaseA.buf[idx] == 0xffffffff) data.PhaseA.idle_count++;
|
|
if (data.PhaseB.buf[idx] == 0x00000000 || data.PhaseB.buf[idx] == 0xffffffff) data.PhaseB.idle_count++;
|
|
}
|
|
} else {
|
|
data.PhaseC.buf[idx] = (data.PhaseC.buf[idx] >> 1) | (bit_a ? 0x80000000 : 0);
|
|
data.PhaseD.buf[idx] = (data.PhaseD.buf[idx] >> 1) | (bit_b ? 0x80000000 : 0);
|
|
data.phase_toggle = 0;
|
|
|
|
if ((data.data_bit_counter & 0xFF) == 0xFF) {
|
|
if (data.PhaseC.buf[idx] == 0x00000000 || data.PhaseC.buf[idx] == 0xffffffff) data.PhaseC.idle_count++;
|
|
if (data.PhaseD.buf[idx] == 0x00000000 || data.PhaseD.buf[idx] == 0xffffffff) data.PhaseD.idle_count++;
|
|
}
|
|
}
|
|
|
|
if (sync.baud == 1600 || data.phase_toggle == 0) {
|
|
data.data_bit_counter++;
|
|
}
|
|
|
|
int idle = 0;
|
|
if (sync.baud == 1600) {
|
|
if (sync.levels == 2) {
|
|
idle = (data.PhaseA.idle_count > IDLE_THRESHOLD);
|
|
} else {
|
|
idle = ((data.PhaseA.idle_count > IDLE_THRESHOLD) && (data.PhaseB.idle_count > IDLE_THRESHOLD));
|
|
}
|
|
} else {
|
|
if (sync.levels == 2) {
|
|
idle = ((data.PhaseA.idle_count > IDLE_THRESHOLD) && (data.PhaseC.idle_count > IDLE_THRESHOLD));
|
|
} else {
|
|
idle = ((data.PhaseA.idle_count > IDLE_THRESHOLD) && (data.PhaseB.idle_count > IDLE_THRESHOLD) && (data.PhaseC.idle_count > IDLE_THRESHOLD) && (data.PhaseD.idle_count > IDLE_THRESHOLD));
|
|
}
|
|
}
|
|
return idle;
|
|
}
|
|
|
|
void FlexProcessor::flex_sym(unsigned char sym) {
|
|
unsigned char sym_rectified;
|
|
if (sync.polarity) {
|
|
sym_rectified = 3 - sym;
|
|
} else {
|
|
sym_rectified = sym;
|
|
}
|
|
|
|
switch (state.Current) {
|
|
case flex::State::SYNC1: {
|
|
unsigned int sync_code = flex_sync(sym);
|
|
if (sync_code != 0) {
|
|
decode_mode(sync_code);
|
|
if (sync.baud != 0 && sync.levels != 0) {
|
|
state.Current = flex::State::FIW;
|
|
send_debug("SYNC1 Found", sync.baud, sync_code);
|
|
} else {
|
|
state.Current = flex::State::SYNC1;
|
|
}
|
|
} else {
|
|
state.Current = flex::State::SYNC1;
|
|
}
|
|
state.fiwcount = 0;
|
|
fiw.rawdata = 0;
|
|
break;
|
|
}
|
|
case flex::State::FIW: {
|
|
state.fiwcount++;
|
|
if (state.fiwcount >= 16) {
|
|
read_2fsk(sym_rectified, &fiw.rawdata);
|
|
}
|
|
if (state.fiwcount == 48) {
|
|
if (decode_fiw() == 0) {
|
|
state.sync2_count = 0;
|
|
state.sync2_shiftreg = 0;
|
|
state.sync2_c_pos = -1;
|
|
state.sync2_cinv_pos = -1;
|
|
demodulator.baud = sync.baud;
|
|
state.Current = flex::State::SYNC2;
|
|
send_debug("FIW OK", fiw.frameno, fiw.cycleno);
|
|
} else {
|
|
state.Current = flex::State::SYNC1;
|
|
send_debug("FIW Fail", fiw.rawdata, 0);
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
case flex::State::SYNC2: {
|
|
/* S2 structure: BS2 + C(16 bits) + inv.BS2 + inv.C(16 bits)
|
|
* Total duration: 25ms at the data symbol rate.
|
|
*
|
|
* We scan for the 16-bit C pattern (0xED84) using a shift
|
|
* register. If found, we validate timing. If not found,
|
|
* we fall back to the nominal 25ms skip (current behavior).
|
|
*
|
|
* Only the MSB (bit_a) matters for C detection - it's a
|
|
* 2-level pattern even in 4FSK modes. */
|
|
unsigned char s2_sym = sync.polarity ? (3 - sym) : sym;
|
|
int bit_a = (s2_sym > 1) ? 1 : 0;
|
|
state.sync2_shiftreg = (state.sync2_shiftreg << 1) | bit_a;
|
|
state.sync2_count++;
|
|
|
|
/* Check for C pattern match (Hamming distance <= 2) */
|
|
if (state.sync2_count >= 16) {
|
|
uint16_t diff_c = state.sync2_shiftreg ^ 0xED84;
|
|
uint16_t diff_cinv = state.sync2_shiftreg ^ 0x127B;
|
|
int errs_c = __builtin_popcount(diff_c);
|
|
int errs_cinv = __builtin_popcount(diff_cinv);
|
|
|
|
if (errs_c <= 2 && state.sync2_c_pos < 0)
|
|
state.sync2_c_pos = (int)state.sync2_count;
|
|
if (errs_cinv <= 2 && state.sync2_cinv_pos < 0)
|
|
state.sync2_cinv_pos = (int)state.sync2_count;
|
|
}
|
|
|
|
/* Nominal S2 duration in symbols */
|
|
unsigned int s2_nominal = sync.baud * 25 / 1000;
|
|
|
|
/* Data starts after inv.C ends. If we detected inv.C,
|
|
* use its position as the true data boundary. Otherwise
|
|
* fall back to the nominal count. */
|
|
unsigned int s2_end = s2_nominal;
|
|
if (state.sync2_cinv_pos > 0) {
|
|
unsigned int cinv_end = (unsigned int)state.sync2_cinv_pos;
|
|
int diff = (int)cinv_end - (int)s2_nominal;
|
|
if (diff >= -1 && diff <= 1)
|
|
s2_end = cinv_end;
|
|
}
|
|
|
|
if (state.sync2_count == s2_end) {
|
|
// Clear phase data
|
|
for (int i = 0; i < 88; i++) {
|
|
data.PhaseA.buf[i] = 0;
|
|
data.PhaseB.buf[i] = 0;
|
|
data.PhaseC.buf[i] = 0;
|
|
data.PhaseD.buf[i] = 0;
|
|
}
|
|
data.PhaseA.idle_count = 0;
|
|
data.PhaseB.idle_count = 0;
|
|
data.PhaseC.idle_count = 0;
|
|
data.PhaseD.idle_count = 0;
|
|
data.phase_toggle = 0;
|
|
data.data_bit_counter = 0;
|
|
state.data_count = 0;
|
|
|
|
state.sync2_shiftreg = 0;
|
|
state.sync2_c_pos = -1;
|
|
state.sync2_cinv_pos = -1;
|
|
|
|
state.Current = flex::State::DATA;
|
|
}
|
|
/* Safety: don't get stuck past nominal */
|
|
if (state.sync2_count > s2_nominal + 1) {
|
|
state.sync2_shiftreg = 0;
|
|
state.sync2_c_pos = -1;
|
|
state.sync2_cinv_pos = -1;
|
|
state.Current = flex::State::SYNC1;
|
|
}
|
|
break;
|
|
}
|
|
case flex::State::DATA: {
|
|
int idle = read_data(sym_rectified);
|
|
if (++state.data_count == sync.baud * 1760 / 1000 || idle) {
|
|
decode_data();
|
|
demodulator.baud = 1600;
|
|
state.Current = flex::State::SYNC1;
|
|
state.data_count = 0;
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
void FlexProcessor::decode_data() {
|
|
if (sync.baud == 1600) {
|
|
if (sync.levels == 2) {
|
|
decode_phase('A');
|
|
} else {
|
|
decode_phase('A');
|
|
decode_phase('B');
|
|
}
|
|
} else {
|
|
if (sync.levels == 2) {
|
|
decode_phase('A');
|
|
decode_phase('C');
|
|
} else {
|
|
decode_phase('A');
|
|
decode_phase('B');
|
|
decode_phase('C');
|
|
decode_phase('D');
|
|
}
|
|
}
|
|
}
|
|
|
|
void FlexProcessor::decode_phase(char PhaseNo) {
|
|
uint32_t* phaseptr = nullptr;
|
|
switch (PhaseNo) {
|
|
case 'A':
|
|
phaseptr = data.PhaseA.buf;
|
|
break;
|
|
case 'B':
|
|
phaseptr = data.PhaseB.buf;
|
|
break;
|
|
case 'C':
|
|
phaseptr = data.PhaseC.buf;
|
|
break;
|
|
case 'D':
|
|
phaseptr = data.PhaseD.buf;
|
|
break;
|
|
default:
|
|
return;
|
|
}
|
|
|
|
/* Check if phase is all idle BEFORE BCH correction.
|
|
* Idle fill uses alternating 0xFFFFFFFF and 0x00000000 words.
|
|
* If every word is one of these two patterns, the phase has no
|
|
* real data - skip it to avoid BCH "correcting" idle into garbage. */
|
|
{
|
|
int all_idle = 1;
|
|
for (int i = 0; i < 88; i++) {
|
|
if (phaseptr[i] != 0xFFFFFFFF && phaseptr[i] != 0x00000000) {
|
|
all_idle = 0;
|
|
break;
|
|
}
|
|
}
|
|
if (all_idle) return;
|
|
}
|
|
|
|
/* BCH decode each word. Mark uncorrectable words but continue. */
|
|
uint8_t word_bad[88] = {0};
|
|
for (int i = 0; i < 88; i++) {
|
|
int decode_error = bch_fix_errors(&phaseptr[i]);
|
|
if (decode_error > 2) {
|
|
word_bad[i] = 1;
|
|
phaseptr[i] = 0;
|
|
}
|
|
phaseptr[i] &= 0x001FFFFF;
|
|
}
|
|
|
|
/* BIW must be good to proceed */
|
|
if (word_bad[0]) return;
|
|
|
|
uint32_t biw = phaseptr[0];
|
|
if (biw == 0 || biw == 0x001FFFFF) return;
|
|
|
|
int voffset = (biw >> 10) & 0x3f;
|
|
int aoffset = ((biw >> 8) & 0x03) + 1;
|
|
int prio_count = (biw >> 4) & 0x0F; // number of priority address words
|
|
|
|
if (voffset < aoffset || voffset >= 88) return;
|
|
|
|
/* Always send BIW1 packet so the app knows we decoded a frame.
|
|
* This updates the status bar even for idle frames. */
|
|
{
|
|
flex::FlexPacket bpkt{};
|
|
bpkt.type = 9;
|
|
bpkt.bitrate = sync.baud * (sync.levels == 4 ? 2 : 1);
|
|
bpkt.cycle = fiw.cycleno;
|
|
bpkt.frame = fiw.frameno;
|
|
bpkt.phase = PhaseNo;
|
|
bpkt.is_inverted = sync.polarity;
|
|
bpkt.fiw_roaming = fiw.roaming;
|
|
bpkt.function = 0;
|
|
bpkt.biw_field = 0xFF;
|
|
bpkt.message[0] = '\0';
|
|
send_packet(bpkt);
|
|
}
|
|
|
|
/* Parse BIW words (indices 1 through aoffset-1).
|
|
* Each BIW word has a 3-bit type field (bits 4-6) that determines content.
|
|
* Send each as a BIW event packet. */
|
|
for (int bw = 1; bw < aoffset && bw < 88; bw++) {
|
|
if (word_bad[bw]) continue;
|
|
uint32_t bword = phaseptr[bw];
|
|
uint32_t btype = (bword >> 4) & 0x07;
|
|
|
|
/* Skip reserved types (3, 4, 6) */
|
|
if (btype == 3 || btype == 4 || btype == 6) continue;
|
|
|
|
flex::FlexPacket bpkt{};
|
|
bpkt.type = 9; // BIW event
|
|
bpkt.bitrate = sync.baud * (sync.levels == 4 ? 2 : 1);
|
|
bpkt.cycle = fiw.cycleno;
|
|
bpkt.frame = fiw.frameno;
|
|
bpkt.phase = PhaseNo;
|
|
bpkt.is_inverted = sync.polarity;
|
|
bpkt.fiw_roaming = fiw.roaming;
|
|
bpkt.function = bw; // BIW word index
|
|
bpkt.biw_field = btype; // BIW type (0,1,2,5,7)
|
|
bpkt.message[0] = '\0';
|
|
|
|
switch (btype) {
|
|
case 0: // SSID1: v1=lid, v2=cz
|
|
bpkt.biw_v1 = (bword >> 12) & 0x01FF;
|
|
bpkt.biw_v2 = (bword >> 7) & 0x1F;
|
|
break;
|
|
case 1: // Date: v1=year(+1994), v2=month, v3=day
|
|
bpkt.biw_v1 = ((bword >> 7) & 0x1F) + 1994;
|
|
bpkt.biw_v2 = (bword >> 17) & 0x0F;
|
|
bpkt.biw_v3 = (bword >> 12) & 0x1F;
|
|
break;
|
|
case 2: // Time: v1=hour, v2=minute, v3=sec_raw(0-7)
|
|
bpkt.biw_v1 = (bword >> 7) & 0x1F;
|
|
bpkt.biw_v2 = (bword >> 12) & 0x3F;
|
|
bpkt.biw_v3 = (bword >> 18) & 0x07;
|
|
break;
|
|
case 5: // SysInfo: v1=a_type, v2=info(10 bits)
|
|
bpkt.biw_v1 = (bword >> 7) & 0x0F;
|
|
bpkt.biw_v2 = (bword >> 11) & 0x03FF;
|
|
break;
|
|
case 7: // SSID2: v1=country, v2=tmf
|
|
bpkt.biw_v1 = (bword >> 11) & 0x03FF;
|
|
bpkt.biw_v2 = (bword >> 7) & 0x0F;
|
|
break;
|
|
default:
|
|
continue;
|
|
}
|
|
send_packet(bpkt);
|
|
}
|
|
|
|
/* Pre-scan: count valid vector words using 4-bit nibble checksum.
|
|
* Tone-only addresses sit at the end of the address field with no
|
|
* corresponding vector. We find the last vector that passes checksum.
|
|
* Note: for long addresses, the 2nd vector word (Vy) is a message word
|
|
* that won't pass checksum - so we count all passing words, not just
|
|
* consecutive ones from the start. */
|
|
int n_valid_vecs = 0;
|
|
for (int vi = 0; vi < (voffset - aoffset); vi++) {
|
|
int wi = voffset + vi;
|
|
if (wi >= 88) break;
|
|
uint32_t vw = phaseptr[wi];
|
|
uint32_t csum = (vw & 0xF) + ((vw >> 4) & 0xF) + ((vw >> 8) & 0xF) +
|
|
((vw >> 12) & 0xF) + ((vw >> 16) & 0xF) + ((vw >> 20) & 0x1);
|
|
if ((csum & 0xF) == 0xF)
|
|
n_valid_vecs = vi + 1; // track highest passing index + 1
|
|
}
|
|
|
|
/* No addresses if voffset == aoffset */
|
|
if (voffset <= aoffset) return;
|
|
|
|
int vec_count = 0;
|
|
int addr_count = 0; // tracks address word position for priority detection
|
|
for (int i = aoffset; i < voffset; i++) {
|
|
int j = voffset + vec_count;
|
|
if (j >= 88) break;
|
|
if (phaseptr[i] == 0x00000000 || phaseptr[i] == 0x001FFFFF) continue;
|
|
|
|
/* Address word - all 21 information bits are address data
|
|
* per 3.8.2. Address type is determined by value range
|
|
* (Table 3.8.1-1). Temporary addresses are range
|
|
* 0x1F7800-0x1F780F (3.8.2.3), identified via addr_type. */
|
|
int is_priority = (addr_count < prio_count) ? 1 : 0;
|
|
|
|
parse_capcode(phaseptr[i]);
|
|
decode.is_priority = is_priority;
|
|
addr_count++;
|
|
|
|
if (decode.long_address) {
|
|
/* Long address: 2 address words, 2 vector words.
|
|
* Read second address word and compute capcode from set. */
|
|
if (i + 1 >= voffset) break; // truncated
|
|
uint32_t aw1 = phaseptr[i];
|
|
uint32_t aw2 = phaseptr[i + 1];
|
|
if (aw2 == 0x00000000 || aw2 == 0x001FFFFF) {
|
|
i++;
|
|
addr_count++; // second address word counts
|
|
vec_count += 2;
|
|
continue;
|
|
}
|
|
|
|
int64_t cap = 0;
|
|
if (aw1 >= 0x000001 && aw1 <= 0x008000 &&
|
|
aw2 >= 0x1F7FFF && aw2 <= 0x1FFFFE) {
|
|
/* Set 1-2 */
|
|
cap = (int64_t)aw1 + (int64_t)(0x1FFFFF - aw2) * 32768LL + 2068480LL;
|
|
} else if (aw1 >= 0x000001 && aw1 <= 0x008000 &&
|
|
aw2 >= 0x1E0001 && aw2 <= 0x1F0000) {
|
|
/* Set 1-3 / 1-4 */
|
|
cap = (int64_t)aw1 + (int64_t)(aw2 - 1933312) * 32768LL + 2068480LL;
|
|
} else if (aw1 >= 0x1F7FFF && aw1 <= 0x1FFFFE &&
|
|
aw2 >= 0x1E0001 && aw2 <= 0x1F0000) {
|
|
/* Set 2-3 */
|
|
cap = (int64_t)(aw1 - 2064383) + (int64_t)(aw2 - 1867776) * 32768LL + 2068479LL;
|
|
} else {
|
|
/* Unknown set - skip */
|
|
i++;
|
|
addr_count++; // second address word counts
|
|
vec_count += 2;
|
|
continue;
|
|
}
|
|
|
|
decode.capcode = cap;
|
|
i++; // consumed 2 address words
|
|
addr_count++; // second address word also counts
|
|
|
|
/* Long addresses always have vectors - they cannot be tone-only.
|
|
* (Tone-only is only for short addresses at the end of AF.)
|
|
* The second vector word (Vy) contains the first message word,
|
|
* not a checksummed vector, so skip the pre-scan check here. */
|
|
vec_count += 2; // consumed 2 vector words
|
|
j = voffset + vec_count - 2; // point to first vector word of pair
|
|
} else {
|
|
if (decode.capcode > 4297068542ll || decode.capcode <= 0) continue;
|
|
|
|
/* Tone-only: address beyond valid vector range */
|
|
if (vec_count >= n_valid_vecs) {
|
|
parse_tone_only(phaseptr, PhaseNo, 0);
|
|
continue;
|
|
}
|
|
vec_count++;
|
|
}
|
|
|
|
uint32_t viw = phaseptr[j];
|
|
int type_val = (viw >> 4) & 0x07;
|
|
|
|
switch (type_val) {
|
|
case 0:
|
|
decode.type = flex::PageType::SECURE;
|
|
break;
|
|
case 1:
|
|
decode.type = flex::PageType::SHORT_INSTRUCTION;
|
|
break;
|
|
case 2:
|
|
decode.type = flex::PageType::TONE;
|
|
break;
|
|
case 3:
|
|
decode.type = flex::PageType::STANDARD_NUMERIC;
|
|
break;
|
|
case 4:
|
|
decode.type = flex::PageType::SPECIAL_NUMERIC;
|
|
break;
|
|
case 5:
|
|
decode.type = flex::PageType::ALPHANUMERIC;
|
|
break;
|
|
case 6:
|
|
decode.type = flex::PageType::BINARY;
|
|
break;
|
|
case 7:
|
|
decode.type = flex::PageType::NUMBERED_NUMERIC;
|
|
break;
|
|
}
|
|
|
|
int mw1 = (viw >> 7) & 0x7F;
|
|
int len;
|
|
/* Numeric types (3, 4, 7) have a 3-bit n field (bits 14-16)
|
|
* encoding word_count - 1. Bits 17-20 are the K checksum.
|
|
* Alpha/hex/secure types use the full 7-bit field (bits 14-20). */
|
|
if (type_val == 3 || type_val == 4 || type_val == 7)
|
|
len = ((viw >> 14) & 0x07) + 1;
|
|
else
|
|
len = (viw >> 14) & 0x7F;
|
|
int mw2 = mw1 + (len - 1);
|
|
|
|
if (mw1 == 0 && mw2 == 0) continue;
|
|
if (decode.type == flex::PageType::TONE) mw1 = mw2 = 0;
|
|
|
|
if (decode.type == flex::PageType::ALPHANUMERIC || decode.type == flex::PageType::SECURE) {
|
|
if (mw1 > 87 || mw2 > 87) continue;
|
|
if (decode.long_address) {
|
|
/* For long addresses, body[0] (header with K,C,F,N,R,M) is at
|
|
* Vy (j+1), not at mw1. The vector's mw1 points to body[1]
|
|
* in the message field, and len includes body[0].
|
|
* parse_alphanumeric expects mw1 = header word index (it does
|
|
* mw1++ internally to skip header). So pass mw1-1 so the
|
|
* skip lands on mw1 (first real data word). */
|
|
parse_alphanumeric(phaseptr, word_bad, PhaseNo, mw1 - 1, mw2 - 1, 0);
|
|
} else {
|
|
parse_alphanumeric(phaseptr, word_bad, PhaseNo, mw1, mw2, 0);
|
|
}
|
|
} else if (decode.type == flex::PageType::STANDARD_NUMERIC || decode.type == flex::PageType::SPECIAL_NUMERIC || decode.type == flex::PageType::NUMBERED_NUMERIC) {
|
|
parse_numeric(phaseptr, word_bad, PhaseNo, j);
|
|
} else if (decode.type == flex::PageType::TONE) {
|
|
/* Vector type 2: Short Message (3.9.2).
|
|
* Sub-type t1t0 in bits 7-8, data d0-d11 in bits 9-20. */
|
|
uint32_t t = (viw >> 7) & 0x03;
|
|
uint32_t d = (viw >> 9) & 0x0FFF;
|
|
|
|
flex::FlexPacket packet{};
|
|
packet.bitrate = sync.baud * (sync.levels == 4 ? 2 : 1);
|
|
packet.capcode = decode.capcode;
|
|
packet.function = t;
|
|
packet.cycle = fiw.cycleno;
|
|
packet.frame = fiw.frameno;
|
|
packet.phase = PhaseNo;
|
|
packet.is_inverted = sync.polarity;
|
|
packet.fiw_roaming = fiw.roaming;
|
|
packet.addr_type = static_cast<uint8_t>(decode.addr_type);
|
|
packet.is_priority = decode.is_priority;
|
|
packet.type = 8; // SHORT
|
|
|
|
if (t == 0 && d == 0xCCC) {
|
|
/* Tone-only: all digits are space (0xC) per STD-43A
|
|
* Table 3.9.2-1 note. For long addresses, also check Vy. */
|
|
bool tone = true;
|
|
if (decode.long_address && j + 1 < 88) {
|
|
uint32_t vy = phaseptr[j + 1] & 0xFFFFF;
|
|
if (vy != 0xCCCCC) tone = false;
|
|
}
|
|
if (tone)
|
|
strcpy(packet.message, "TONE");
|
|
else
|
|
goto short_numeric;
|
|
} else if (t == 0) {
|
|
short_numeric:
|
|
/* Numeric: 3 BCD digits from Vx (d0-d11).
|
|
* Long addresses: 5 more digits from Vy (d12-d31),
|
|
* 8 digits total. d32 is spare (set to 0). */
|
|
char *p = packet.message, *e = p + sizeof(packet.message);
|
|
p = str_append(p, e, "NUM ");
|
|
*p++ = flex_bcd[(d >> 0) & 0xF];
|
|
*p++ = flex_bcd[(d >> 4) & 0xF];
|
|
*p++ = flex_bcd[(d >> 8) & 0xF];
|
|
if (decode.long_address && j + 1 < 88) {
|
|
uint32_t vy = phaseptr[j + 1];
|
|
*p++ = flex_bcd[(vy >> 0) & 0xF];
|
|
*p++ = flex_bcd[(vy >> 4) & 0xF];
|
|
*p++ = flex_bcd[(vy >> 8) & 0xF];
|
|
*p++ = flex_bcd[(vy >> 12) & 0xF];
|
|
*p++ = flex_bcd[(vy >> 16) & 0xF];
|
|
}
|
|
*p = '\0';
|
|
} else if (t == 1) {
|
|
/* Source: S2S1S0 in d0-d2 */
|
|
char *p = packet.message, *e = p + sizeof(packet.message);
|
|
p = str_append(p, e, "SRC ");
|
|
str_uint(p, e, d & 0x07);
|
|
} else if (t == 2) {
|
|
/* Numbered: S(3) + N(6) + R(1) */
|
|
uint32_t src = d & 0x07;
|
|
uint32_t n = (d >> 3) & 0x3F;
|
|
uint32_t r = (d >> 9) & 0x01;
|
|
char *p = packet.message, *e = p + sizeof(packet.message);
|
|
p = str_append(p, e, "SRC ");
|
|
p = str_uint(p, e, src);
|
|
p = str_append(p, e, " N=");
|
|
p = str_uint(p, e, n);
|
|
p = str_append(p, e, " R=");
|
|
str_uint(p, e, r);
|
|
} else {
|
|
/* Reserved */
|
|
char *p = packet.message, *e = p + sizeof(packet.message);
|
|
p = str_append(p, e, "RESERVED ");
|
|
str_hex(p, e, d, 3);
|
|
}
|
|
send_packet(packet);
|
|
} else if (decode.type == flex::PageType::BINARY) {
|
|
/* HEX/Binary message.
|
|
* Word 1 (mw1): K(12) C(1) F(2) N(6) = header
|
|
* Word 2 (mw1+1, first frag only): R(1) M(1) D(1) H(1) B(4) I(1) rsvd(4) S(8)
|
|
* Words 3+: data */
|
|
if (mw1 > 87 || mw2 > 87) continue;
|
|
|
|
/* Extract header from word 1 */
|
|
uint8_t hex_c = 0, hex_f = 0, hex_n = 0;
|
|
int hex_hdr_valid = 0;
|
|
if (!word_bad[mw1]) {
|
|
uint32_t hw1 = phaseptr[mw1];
|
|
hex_c = (hw1 >> 12) & 0x01;
|
|
hex_f = (hw1 >> 13) & 0x03;
|
|
hex_n = (hw1 >> 15) & 0x3F;
|
|
hex_hdr_valid = 1;
|
|
}
|
|
|
|
/* Extract word 2 flags (first fragment: F=3) */
|
|
uint8_t hex_r = 0, hex_m = 0, hex_d = 0, hex_b = 0;
|
|
int data_start = mw1 + 1; // default: data starts after header
|
|
if (hex_f == 3 && (mw1 + 1) <= mw2 && !word_bad[mw1 + 1]) {
|
|
uint32_t hw2 = phaseptr[mw1 + 1];
|
|
hex_r = (hw2 >> 0) & 0x01;
|
|
hex_m = (hw2 >> 1) & 0x01;
|
|
hex_d = (hw2 >> 2) & 0x01;
|
|
hex_b = (hw2 >> 4) & 0x0F;
|
|
data_start = mw1 + 2; // skip both header words
|
|
}
|
|
|
|
/* Dump data words as hex */
|
|
char message[256] = {0};
|
|
char *mp = message, *me = message + 250;
|
|
for (int w = data_start; w <= mw2 && mp < me; w++) {
|
|
if (word_bad[w]) {
|
|
mp = str_append(mp, me, "?????? ");
|
|
} else {
|
|
mp = str_hex(mp, me, phaseptr[w] & 0x1FFFFF, 5);
|
|
if (mp < me) *mp++ = ' ';
|
|
*mp = '\0';
|
|
}
|
|
}
|
|
if (mp > message && *(mp - 1) == ' ') {
|
|
mp--;
|
|
*mp = '\0';
|
|
}
|
|
int pos = (int)(mp - message);
|
|
|
|
flex::FlexPacket packet{};
|
|
packet.bitrate = sync.baud * (sync.levels == 4 ? 2 : 1);
|
|
packet.capcode = decode.capcode;
|
|
packet.function = 0;
|
|
packet.type = 6; // HEX
|
|
packet.status = 0;
|
|
packet.cycle = fiw.cycleno;
|
|
packet.frame = fiw.frameno;
|
|
packet.phase = PhaseNo;
|
|
packet.is_inverted = sync.polarity;
|
|
packet.fiw_roaming = fiw.roaming;
|
|
packet.addr_type = static_cast<uint8_t>(decode.addr_type);
|
|
packet.is_priority = decode.is_priority;
|
|
if (hex_hdr_valid) {
|
|
packet.frag = hex_f;
|
|
packet.more_frag = hex_c;
|
|
packet.seq = hex_n;
|
|
packet.has_flags = 1;
|
|
if (hex_f == 3) {
|
|
packet.is_new = hex_r;
|
|
packet.maildrop = hex_m;
|
|
/* Store b and d in function field: low nibble=b, bit4=d */
|
|
packet.function = (hex_d << 4) | hex_b;
|
|
}
|
|
}
|
|
memcpy(packet.message, message, pos + 1);
|
|
send_packet(packet);
|
|
} else if (decode.type == flex::PageType::SHORT_INSTRUCTION) {
|
|
/* Short instruction: 14-bit data in vector bits 7-20.
|
|
* i2i1i0 (bits 0-2 of data) = instruction type.
|
|
* Remaining bits = instruction-specific data. */
|
|
uint32_t instr_data = (viw >> 7) & 0x3FFF;
|
|
uint32_t itype = instr_data & 0x07;
|
|
|
|
flex::FlexPacket packet{};
|
|
packet.bitrate = sync.baud * (sync.levels == 4 ? 2 : 1);
|
|
packet.capcode = decode.capcode;
|
|
packet.function = 0;
|
|
packet.type = 1; // INS
|
|
packet.cycle = fiw.cycleno;
|
|
packet.frame = fiw.frameno;
|
|
packet.phase = PhaseNo;
|
|
packet.is_inverted = sync.polarity;
|
|
packet.fiw_roaming = fiw.roaming;
|
|
packet.addr_type = static_cast<uint8_t>(decode.addr_type);
|
|
packet.is_priority = decode.is_priority;
|
|
|
|
if (itype == 0) {
|
|
uint32_t tgt_frame = (instr_data >> 3) & 0x7F;
|
|
uint32_t slot = (instr_data >> 10) & 0x0F;
|
|
packet.biw_v1 = slot;
|
|
packet.biw_v2 = tgt_frame;
|
|
{
|
|
char *p = packet.message, *e = p + sizeof(packet.message);
|
|
p = str_append(p, e, "i=temp|slot=");
|
|
p = str_uint(p, e, slot);
|
|
p = str_append(p, e, "|target=");
|
|
str_uint(p, e, tgt_frame);
|
|
}
|
|
} else if (itype == 1) {
|
|
uint32_t flags = (instr_data >> 3) & 0x7FF;
|
|
{
|
|
char *p = packet.message, *e = p + sizeof(packet.message);
|
|
p = str_append(p, e, "i=event|flags=");
|
|
str_hex(p, e, flags, 3);
|
|
}
|
|
} else {
|
|
{
|
|
char *p = packet.message, *e = p + sizeof(packet.message);
|
|
p = str_append(p, e, "i=rsvd|type=");
|
|
p = str_uint(p, e, itype);
|
|
p = str_append(p, e, "|raw=");
|
|
str_hex(p, e, instr_data, 4);
|
|
}
|
|
}
|
|
send_packet(packet);
|
|
}
|
|
}
|
|
}
|
|
|
|
void FlexProcessor::parse_capcode(uint32_t aw1) {
|
|
/* Classify address word by range. */
|
|
decode.long_address = 0;
|
|
decode.addr_type = flex::AddrType::SHORT;
|
|
|
|
if ((aw1 >= 0x000001 && aw1 <= 0x008000) || /* LA1 */
|
|
(aw1 >= 0x1E0001 && aw1 <= 0x1E8000) || /* LA3 */
|
|
(aw1 >= 0x1E8001 && aw1 <= 0x1F0000) || /* LA4 */
|
|
(aw1 >= 0x1F7FFF && aw1 <= 0x1FFFFE)) { /* LA2 */
|
|
decode.long_address = 1;
|
|
decode.addr_type = flex::AddrType::LONG;
|
|
} else if (aw1 >= 0x1F7800 && aw1 <= 0x1F780F) {
|
|
decode.addr_type = flex::AddrType::TEMPORARY;
|
|
} else if (aw1 >= 0x1F7810 && aw1 <= 0x1F781F) {
|
|
decode.addr_type = flex::AddrType::OPERATOR;
|
|
} else if (aw1 >= 0x1F6800 && aw1 <= 0x1F77FF) {
|
|
decode.addr_type = flex::AddrType::NETWORK;
|
|
} else if (aw1 >= 0x1F2800 && aw1 <= 0x1F67FF) {
|
|
decode.addr_type = flex::AddrType::INFO_SVC;
|
|
} else if ((aw1 >= 0x1F0001 && aw1 <= 0x1F27FF) ||
|
|
(aw1 >= 0x1F7820 && aw1 <= 0x1F7FFE)) {
|
|
decode.addr_type = flex::AddrType::RESERVED;
|
|
} else if (aw1 >= 0x008001 && aw1 <= 0x1E0000) {
|
|
decode.addr_type = flex::AddrType::SHORT;
|
|
} else {
|
|
decode.addr_type = flex::AddrType::UNKNOWN;
|
|
}
|
|
|
|
decode.capcode = aw1 - 0x8000;
|
|
}
|
|
|
|
void FlexProcessor::parse_alphanumeric(uint32_t* phaseptr, const uint8_t* word_bad, char PhaseNo, int mw1, int mw2, int) {
|
|
char message[256] = {0};
|
|
int currentChar = 0;
|
|
|
|
/* First message word is the header (K, C, F, N, R, M fields).
|
|
* Extract flags before skipping to content. */
|
|
uint8_t hdr_c = 0, hdr_f = 0, hdr_n = 0, hdr_r = 0, hdr_m = 0;
|
|
uint8_t hdr_sig = 0;
|
|
int hdr_valid = 0;
|
|
if (mw1 >= 0 && mw1 < 88 && !word_bad[mw1]) {
|
|
uint32_t hdr = phaseptr[mw1];
|
|
hdr_c = (hdr >> 10) & 0x01; // bit 10
|
|
hdr_f = (hdr >> 11) & 0x03; // bits 11-12
|
|
hdr_n = (hdr >> 13) & 0x3F; // bits 13-18
|
|
hdr_r = (hdr >> 19) & 0x01; // bit 19
|
|
hdr_m = (hdr >> 20) & 0x01; // bit 20
|
|
hdr_valid = 1;
|
|
}
|
|
mw1++;
|
|
|
|
/* Extract signature from first data word (bits 0-6) */
|
|
if (mw1 >= 0 && mw1 < 88 && !word_bad[mw1]) {
|
|
hdr_sig = phaseptr[mw1] & 0x7F;
|
|
}
|
|
|
|
for (int i = mw1; i <= mw2; i++) {
|
|
unsigned int dw = phaseptr[i];
|
|
unsigned char ch;
|
|
int bad = (i >= 0 && i < 88) ? word_bad[i] : 1;
|
|
|
|
if (i > mw1) {
|
|
ch = dw & 0x7F;
|
|
if (bad) {
|
|
if (currentChar < 255) message[currentChar++] = '?';
|
|
} else if (ch >= 0x20 || ch == 0x0A || ch == 0x0D) {
|
|
if (currentChar < 255) message[currentChar++] = ch;
|
|
} else if (ch == 0x03 || ch == 0x00) {
|
|
if (currentChar < 255) message[currentChar++] = '\x03';
|
|
}
|
|
}
|
|
|
|
ch = (dw >> 7) & 0x7F;
|
|
if (bad) {
|
|
if (currentChar < 255) message[currentChar++] = '?';
|
|
} else if (ch >= 0x20 || ch == 0x0A || ch == 0x0D) {
|
|
if (currentChar < 255) message[currentChar++] = ch;
|
|
} else if (ch == 0x03 || ch == 0x00) {
|
|
if (currentChar < 255) message[currentChar++] = '\x03';
|
|
}
|
|
|
|
ch = (dw >> 14) & 0x7F;
|
|
if (bad) {
|
|
if (currentChar < 255) message[currentChar++] = '?';
|
|
} else if (ch >= 0x20 || ch == 0x0A || ch == 0x0D) {
|
|
if (currentChar < 255) message[currentChar++] = ch;
|
|
} else if (ch == 0x03 || ch == 0x00) {
|
|
if (currentChar < 255) message[currentChar++] = '\x03';
|
|
}
|
|
}
|
|
|
|
/* Post-process: trim trailing ETX/NUL padding, but if printable chars
|
|
* appear after an ETX/NUL, show each ETX/NUL as '?' (invalid char). */
|
|
{
|
|
/* First find the last printable character */
|
|
int last_printable = -1;
|
|
for (int k = 0; k < currentChar; k++) {
|
|
if (message[k] != '\x03') last_printable = k;
|
|
}
|
|
/* Now output up to last_printable, replacing ETX with '?' */
|
|
int out = 0;
|
|
for (int k = 0; k <= last_printable && out < 255; k++) {
|
|
if (message[k] == '\x03')
|
|
message[out++] = '?';
|
|
else
|
|
message[out++] = message[k];
|
|
}
|
|
currentChar = out;
|
|
}
|
|
message[currentChar] = '\0';
|
|
|
|
flex::FlexPacket packet{};
|
|
packet.bitrate = sync.baud * (sync.levels == 4 ? 2 : 1);
|
|
packet.capcode = decode.capcode;
|
|
packet.function = 0;
|
|
packet.type = (decode.type == flex::PageType::SECURE) ? 0 : 5;
|
|
packet.status = 0;
|
|
packet.cycle = fiw.cycleno;
|
|
packet.frame = fiw.frameno;
|
|
packet.phase = PhaseNo;
|
|
packet.is_inverted = sync.polarity;
|
|
packet.fiw_roaming = fiw.roaming;
|
|
packet.addr_type = static_cast<uint8_t>(decode.addr_type);
|
|
packet.is_priority = decode.is_priority;
|
|
if (hdr_valid) {
|
|
packet.frag = hdr_f;
|
|
packet.more_frag = hdr_c;
|
|
packet.seq = hdr_n;
|
|
packet.is_new = hdr_r;
|
|
packet.maildrop = hdr_m;
|
|
packet.sig = hdr_sig;
|
|
packet.has_flags = 1;
|
|
if (decode.type == flex::PageType::SECURE) {
|
|
/* Secure: bits 19-20 are t1t0 (encoding type), not R/M */
|
|
packet.sec_enc = (hdr_r) | (hdr_m << 1); // t0=bit19, t1=bit20
|
|
packet.is_new = 0;
|
|
packet.maildrop = 0;
|
|
}
|
|
}
|
|
memcpy(packet.message, message, currentChar + 1);
|
|
|
|
send_packet(packet);
|
|
}
|
|
|
|
void FlexProcessor::parse_numeric(uint32_t* phaseptr, const uint8_t* word_bad, char PhaseNo, int j) {
|
|
char message[256] = {0};
|
|
|
|
/* Extract NNUM header fields from first message word if applicable.
|
|
* Layout: K5K4(2) + N0-N5(6) + R0(1) + S0(1) + BCD digits... */
|
|
uint8_t nnum_n = 0, nnum_r = 0, nnum_s = 0;
|
|
int is_nnum = (decode.type == flex::PageType::NUMBERED_NUMERIC);
|
|
|
|
int w1 = phaseptr[j] >> 7;
|
|
int w2 = w1 >> 7;
|
|
w1 = w1 & 0x7f;
|
|
int n_field = w2 & 0x07; // word_count - 1
|
|
w2 = n_field + w1;
|
|
|
|
// Bounds check: phase buffer is 88 words (indices 0-87)
|
|
if (w1 > 87) return;
|
|
if (w2 > 87) w2 = 87;
|
|
|
|
/* For long addresses (3.9.1):
|
|
* 1-word: b field points to Vy. body[0] at w1.
|
|
* Multi-word: body[0] at Vy (j+1). b field points to MF body[1]. */
|
|
int body0_idx;
|
|
if (decode.long_address && n_field > 0)
|
|
body0_idx = j + 1; // Vy = 2nd vector word
|
|
else
|
|
body0_idx = w1;
|
|
if (body0_idx < 0 || body0_idx >= 88) return;
|
|
|
|
int dw = phaseptr[body0_idx];
|
|
|
|
if (is_nnum) {
|
|
/* Extract N, R, S from the first message word's BCD stream.
|
|
* After K5K4 (2 bits), next 6 bits = N, then R, then S.
|
|
* These are consumed by the skip count (count starts at 4+10=14). */
|
|
nnum_n = (dw >> 2) & 0x3F; // bits 2-7
|
|
nnum_r = (dw >> 8) & 0x01; // bit 8
|
|
nnum_s = (dw >> 9) & 0x01; // bit 9
|
|
}
|
|
|
|
unsigned char digit = 0;
|
|
int count = 4;
|
|
if (is_nnum)
|
|
count += 10; // skip K5K4(2) + N(6) + R(1) + S(1)
|
|
else
|
|
count += 2; // skip K5K4(2)
|
|
|
|
int idx = 0;
|
|
|
|
/* Phase 1: decode body[0] bits.
|
|
* For short addresses, body[0] is at w1 and we advance to w1+1.
|
|
* For long addresses, body[0] is at Vy (j+1), then we continue from w1. */
|
|
if (word_bad[body0_idx]) {
|
|
/* Uncorrectable word — emit '?' for each digit slot */
|
|
int data_bits = 21 - (count - 4); /* bits available after skip */
|
|
int lost_digits = data_bits / 4;
|
|
while (lost_digits-- > 0 && idx < 255)
|
|
message[idx++] = '?';
|
|
count = 4; /* reset for next word */
|
|
digit = 0;
|
|
} else {
|
|
for (int k = 0; k < 21; k++) {
|
|
digit = (digit >> 1) & 0x0F;
|
|
if (dw & 0x01) digit ^= 0x08;
|
|
dw >>= 1;
|
|
if (--count == 0) {
|
|
if (idx < 255) {
|
|
message[idx++] = flex_bcd[digit];
|
|
}
|
|
count = 4;
|
|
}
|
|
}
|
|
}
|
|
|
|
/* Phase 2: decode remaining body words from MF.
|
|
* Short: body[1..n] at w1+1 .. w2.
|
|
* Long: MF has n_field words at w1 .. w1+n_field-1.
|
|
* (n_field = total_words - 1; body[0] is at Vy, not in MF) */
|
|
int start, end;
|
|
if (decode.long_address) {
|
|
start = w1;
|
|
end = w1 + n_field - 1; // empty when n_field=0
|
|
} else {
|
|
start = w1 + 1;
|
|
end = w2;
|
|
}
|
|
for (int i = start; i <= end && i < 88; i++) {
|
|
if (word_bad[i]) {
|
|
/* Uncorrectable word — emit '?' for each digit slot (5 per word) */
|
|
int lost_digits = 21 / 4; /* 5 digits per 21-bit word */
|
|
while (lost_digits-- > 0 && idx < 255)
|
|
message[idx++] = '?';
|
|
count = 4;
|
|
digit = 0;
|
|
continue;
|
|
}
|
|
dw = phaseptr[i];
|
|
for (int k = 0; k < 21; k++) {
|
|
digit = (digit >> 1) & 0x0F;
|
|
if (dw & 0x01) digit ^= 0x08;
|
|
dw >>= 1;
|
|
if (--count == 0) {
|
|
if (idx < 255) {
|
|
message[idx++] = flex_bcd[digit];
|
|
}
|
|
count = 4;
|
|
}
|
|
}
|
|
}
|
|
|
|
/* Trim trailing BCD space padding (0x0C = ' ').
|
|
* The encoder pads unused nibble slots with 0x0C */
|
|
while (idx > 0 && message[idx - 1] == ' ')
|
|
idx--;
|
|
message[idx] = '\0';
|
|
|
|
flex::FlexPacket packet{};
|
|
packet.bitrate = sync.baud * (sync.levels == 4 ? 2 : 1);
|
|
packet.capcode = decode.capcode;
|
|
packet.function = 0;
|
|
/* Set correct type: 3=NUM, 4=SNUM, 7=NNUM */
|
|
if (decode.type == flex::PageType::SPECIAL_NUMERIC)
|
|
packet.type = 4;
|
|
else if (is_nnum)
|
|
packet.type = 7;
|
|
else
|
|
packet.type = 3;
|
|
packet.status = 0;
|
|
packet.cycle = fiw.cycleno;
|
|
packet.frame = fiw.frameno;
|
|
packet.phase = PhaseNo;
|
|
packet.is_inverted = sync.polarity;
|
|
packet.fiw_roaming = fiw.roaming;
|
|
packet.addr_type = static_cast<uint8_t>(decode.addr_type);
|
|
packet.is_priority = decode.is_priority;
|
|
if (is_nnum) {
|
|
packet.seq = nnum_n;
|
|
packet.is_new = nnum_r;
|
|
packet.nnum_s = nnum_s;
|
|
packet.has_flags = 1;
|
|
}
|
|
memcpy(packet.message, message, idx + 1);
|
|
|
|
send_packet(packet);
|
|
}
|
|
|
|
void FlexProcessor::parse_tone_only(uint32_t*, char PhaseNo, int) {
|
|
if (decode.capcode == 1) return; // idle artifact
|
|
flex::FlexPacket packet{};
|
|
packet.bitrate = sync.baud * (sync.levels == 4 ? 2 : 1);
|
|
packet.capcode = decode.capcode;
|
|
packet.function = 0;
|
|
packet.type = 2; // TONE
|
|
packet.status = 0;
|
|
packet.cycle = fiw.cycleno;
|
|
packet.frame = fiw.frameno;
|
|
packet.phase = PhaseNo;
|
|
packet.is_inverted = sync.polarity;
|
|
packet.fiw_roaming = fiw.roaming;
|
|
packet.addr_type = static_cast<uint8_t>(decode.addr_type);
|
|
packet.is_priority = decode.is_priority;
|
|
strcpy(packet.message, "");
|
|
|
|
send_packet(packet);
|
|
}
|
|
|
|
void FlexProcessor::parse_unknown(uint32_t*, char, int, int) {
|
|
// Ignored
|
|
}
|
|
|
|
void FlexProcessor::on_message(const Message* const message) {
|
|
if (message->id == Message::ID::FlexConfigure) {
|
|
configure();
|
|
}
|
|
}
|
|
|
|
void FlexProcessor::configure() {
|
|
decim_0_iq.configure(taps_11k0_decim_0.taps);
|
|
decim_1_iq.configure(taps_11k0_decim_1.taps);
|
|
channel_filter.configure(taps_11k0_channel.taps, 2); // Decim 2 -> 24kHz output
|
|
|
|
demod.configure(24000, 4800);
|
|
demodulator.sample_freq = 24000;
|
|
|
|
configured = true;
|
|
send_debug("Configured", 0, 0);
|
|
}
|
|
|
|
void FlexProcessor::send_packet(const flex::FlexPacket& packet) {
|
|
FlexPacketMessage message(packet);
|
|
shared_memory.application_queue.push(message);
|
|
}
|
|
|
|
void FlexProcessor::send_stats() {
|
|
// Stats
|
|
}
|
|
|
|
int main() {
|
|
EventDispatcher event_dispatcher{std::make_unique<FlexProcessor>()};
|
|
event_dispatcher.run();
|
|
return 0;
|
|
}
|