331 lines
9.0 KiB
C++
331 lines
9.0 KiB
C++
/* -*- c++ -*- */
|
|
/*
|
|
* Copyright 2004 Free Software Foundation, Inc.
|
|
*
|
|
* This file is part of GNU Radio
|
|
*
|
|
* GNU Radio 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 2, or (at your option)
|
|
* any later version.
|
|
*
|
|
* GNU Radio 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 GNU Radio; see the file COPYING. If not, write to
|
|
* the Free Software Foundation, Inc., 59 Temple Place - Suite 330,
|
|
* Boston, MA 02111-1307, USA.
|
|
*/
|
|
|
|
/*
|
|
* config.h is generated by configure. It contains the results
|
|
* of probing for features, options etc. It should be the first
|
|
* file included in your .cc file.
|
|
*/
|
|
#ifdef HAVE_CONFIG_H
|
|
#include "config.h"
|
|
#endif
|
|
|
|
#include <repeater_p25_frame_fb.h>
|
|
#include <gr_io_signature.h>
|
|
|
|
#include <stdio.h>
|
|
#include <stdlib.h>
|
|
#include <stdint.h>
|
|
#include <string.h>
|
|
#include <vector>
|
|
#include <bch.h>
|
|
#include <op25_imbe_frame.h>
|
|
#include <op25_p25_frame.h>
|
|
#include <sys/time.h>
|
|
|
|
/*
|
|
* Create a new instance of repeater_p25_frame_fb and return
|
|
* a boost shared_ptr. This is effectively the public constructor.
|
|
*/
|
|
repeater_p25_frame_fb_sptr
|
|
repeater_make_p25_frame_fb ()
|
|
{
|
|
return repeater_p25_frame_fb_sptr (new repeater_p25_frame_fb ());
|
|
}
|
|
|
|
/*
|
|
* Specify constraints on number of input and output streams.
|
|
* This info is used to construct the input and output signatures
|
|
* (2nd & 3rd args to gr_block's constructor). The input and
|
|
* output signatures are used by the runtime system to
|
|
* check that a valid number and type of inputs and outputs
|
|
* are connected to this block. In this case, we accept
|
|
* only 1 input and 1 output.
|
|
*/
|
|
static const int MIN_IN = 1; // mininum number of input streams
|
|
static const int MAX_IN = 1; // maximum number of input streams
|
|
static const int MIN_OUT = 1; // minimum number of output streams
|
|
static const int MAX_OUT = 1; // maximum number of output streams
|
|
|
|
/*
|
|
* The private constructor
|
|
*/
|
|
repeater_p25_frame_fb::repeater_p25_frame_fb ()
|
|
: gr_block ("p25_frame_fb",
|
|
gr_make_io_signature (MIN_IN, MAX_IN, sizeof (float)),
|
|
gr_make_io_signature (MIN_OUT, MAX_OUT, sizeof (unsigned char))),
|
|
reverse_p(0),
|
|
hdr_syms(0),
|
|
next_bit(0),
|
|
frame_body(P25_VOICE_FRAME_SIZE),
|
|
symbol_queue(),
|
|
surplus(-864),
|
|
frame_size_limit(0),
|
|
last_sym_ct(0),
|
|
closed_repeater(1),
|
|
stats_sym_ct(0),
|
|
stats_bad_bch(0),
|
|
stats_good_bch(0),
|
|
stats_bch_errs(0),
|
|
stats_e0_errs(0),
|
|
stats_et_errs(0)
|
|
{
|
|
set_history(864); // always behind by sizeof(voice data unit)
|
|
memset(stats_frames_received, 0, sizeof(stats_frames_received));
|
|
memset(closed_nac_list, 0, sizeof(closed_nac_list));
|
|
memset(stats_frame_history, 0, sizeof(stats_frame_history));
|
|
}
|
|
|
|
/*
|
|
* Our virtual destructor.
|
|
*/
|
|
repeater_p25_frame_fb::~repeater_p25_frame_fb ()
|
|
{
|
|
// nothing else required in this example
|
|
}
|
|
|
|
static const int max_frame_lengths[16] = {
|
|
792, // 0 - pdu
|
|
0, 0, // 1, 2 - undef
|
|
144, // 3 - tdu
|
|
0, // 4 - undef
|
|
P25_VOICE_FRAME_SIZE, // 5 - ldu1
|
|
0, // 6 - undef
|
|
0, // 7 - tsbk
|
|
0, // 8 - undef
|
|
0, // 9 - VSELP "voice PDU"
|
|
P25_VOICE_FRAME_SIZE, // a - ldu2
|
|
0, // b - undef
|
|
0, // c - VSELP "voice PDU"
|
|
0, 0, // d, e - undef
|
|
432 // f - tdu
|
|
};
|
|
|
|
/*
|
|
* Record stats. If received NAC matches a previous entry, use it;
|
|
* otherwise overlay least-recent non-matching history entry (unless an empty
|
|
* one exists)
|
|
*/
|
|
void repeater_p25_frame_fb::record_frame(uint32_t nac, uint32_t duid) {
|
|
struct timeval tv;
|
|
struct timezone tz;
|
|
time_t t0=0;
|
|
suseconds_t u0=0;
|
|
int least_recent = 0;
|
|
int emptyp = -1;
|
|
int i;
|
|
|
|
stats_frames_received[duid]++;
|
|
|
|
gettimeofday(&tv, &tz);
|
|
for (i=0; i < FRAME_HISTORY_SIZE; i++) {
|
|
if (stats_frame_history[i].nac == nac) {
|
|
stats_frame_history[i].sec = tv.tv_sec;
|
|
stats_frame_history[i].usec = tv.tv_usec;
|
|
stats_frame_history[i].frame_ctr++;
|
|
return;
|
|
}
|
|
if (stats_frame_history[i].nac == 0) {
|
|
emptyp = i;
|
|
}
|
|
if (t0 == 0 || stats_frame_history[i].sec < t0 ||
|
|
(stats_frame_history[i].sec == t0 &&
|
|
stats_frame_history[i].usec < u0)) {
|
|
t0 = stats_frame_history[i].sec;
|
|
u0 = stats_frame_history[i].usec;
|
|
least_recent = i;
|
|
}
|
|
}
|
|
if (emptyp >= 0)
|
|
i = emptyp;
|
|
else
|
|
i = least_recent;
|
|
stats_frame_history[i].nac = nac;
|
|
stats_frame_history[i].sec = tv.tv_sec;
|
|
stats_frame_history[i].usec = tv.tv_usec;
|
|
stats_frame_history[i].frame_ctr++;
|
|
}
|
|
|
|
/*
|
|
* Process the 64 bits after the frame sync, which should be the frame header
|
|
* 1. verify bch and reject if bch cannot be decoded
|
|
* 2. extract NAC and DUID
|
|
* Returns false if decode failure, else true
|
|
*/
|
|
bool repeater_p25_frame_fb::header_codeword(uint64_t acc, uint32_t& nac, uint32_t& duid) {
|
|
bit_vector cw(64);
|
|
bool low = acc & 1;
|
|
// for bch, split bits into codeword vector
|
|
for (int i=0; i < 64; i++) {
|
|
acc >>= 1;
|
|
cw[i] = acc & 1;
|
|
}
|
|
|
|
// do bch decode
|
|
int rc = bchDec(cw);
|
|
|
|
// check if bch decode unsuccessful
|
|
if (rc < 0) {
|
|
stats_bad_bch ++;
|
|
return false;
|
|
}
|
|
stats_good_bch ++;
|
|
if (rc > 0) {
|
|
// FIXME: stats_bch_err_counters[rc] ++;
|
|
stats_bch_errs ++;
|
|
}
|
|
|
|
// load corrected bch bits into acc
|
|
acc = 0;
|
|
for (int i=63; i>=0; i--) {
|
|
acc |= cw[i];
|
|
acc <<= 1;
|
|
}
|
|
acc |= low; // FIXME
|
|
|
|
// extract nac and duid
|
|
nac = (acc >> 52) & 0xfff;
|
|
duid = (acc >> 48) & 0x00f;
|
|
hdr_word = acc;
|
|
record_frame(nac, duid);
|
|
printf ("nac %x duid %x errs %d sym %llu [%llu]\n", nac, duid, rc, stats_sym_ct, stats_sym_ct - last_sym_ct);
|
|
last_sym_ct = stats_sym_ct;
|
|
|
|
return true;
|
|
}
|
|
|
|
void repeater_p25_frame_fb::rebuild_frame(bit_vector& frame_body){
|
|
if (duid == 0x5 || duid == 0xa) {
|
|
imbe_regenerate_voice_unit(frame_body);
|
|
}
|
|
// FIXME: handle other frame types
|
|
}
|
|
|
|
/*
|
|
* rx_sym: called once per received symbol
|
|
* 1. looks for flags sequences
|
|
* 2. after flags detected (hdr_syms > 0), accumulate 64-bit header word
|
|
* 3. check for voice frames (ldu1|2)
|
|
* 4. for ldu1|2 (next_bit > 0), accumulate all 1728 bits in frame
|
|
* 5. FIXME regenerate all fields in frame (flags, bch, hamming/golay, rs, pn)
|
|
* 6. place symbols into output symbol queue
|
|
*/
|
|
void repeater_p25_frame_fb::rx_sym(uint8_t dibit) {
|
|
// FIXME assert(dibit >= 0 && dibit <= 3)
|
|
header_accum <<= 2;
|
|
header_accum |= dibit;
|
|
if (hdr_syms == 12) {
|
|
// ignore status dibit
|
|
header_accum >>= 2;
|
|
} else if (hdr_syms >= 33) {
|
|
// header completely received
|
|
bool bch_rc = header_codeword(header_accum, nac, duid);
|
|
// next_bit = 48 + 64 + 2;
|
|
if (bch_rc) {
|
|
next_bit = 48 + 64;
|
|
frame_size_limit = max_frame_lengths[duid];
|
|
if (frame_size_limit <= next_bit)
|
|
frame_size_limit = P25_VOICE_FRAME_SIZE;
|
|
}
|
|
hdr_syms = 0;
|
|
}
|
|
if (hdr_syms)
|
|
// count symbols in header
|
|
hdr_syms++;
|
|
if ((header_accum & P25_FRAME_SYNC_MASK) == P25_FRAME_SYNC_MAGIC) {
|
|
hdr_syms = 1;
|
|
}
|
|
if ((header_accum & P25_FRAME_SYNC_MASK) == P25_FRAME_SYNC_REV_P) {
|
|
reverse_p = 0x02;
|
|
hdr_syms = 1;
|
|
}
|
|
if (next_bit) {
|
|
frame_body[next_bit++] = (dibit >> 1) & 1;
|
|
frame_body[next_bit++] = dibit & 1;
|
|
}
|
|
// dispose of received frame (if exists) and:
|
|
// 1. complete frame is received, or
|
|
// 2. flags is received
|
|
if (next_bit && (next_bit >= frame_size_limit || hdr_syms == 1)) {
|
|
// dump_cw(frame_body, 216, stdout);
|
|
rebuild_frame(frame_body);
|
|
p25_setup_frame_header(frame_body, hdr_word);
|
|
for (uint32_t i = 0; i < next_bit; i += 2) {
|
|
uint8_t new_dibit = (frame_body[i] << 1) +
|
|
frame_body[i+1];
|
|
symbol_queue.push_back(new_dibit);
|
|
}
|
|
next_bit = 0;
|
|
}
|
|
++stats_sym_ct;
|
|
}
|
|
|
|
/*
|
|
* Slicer for normalized baseband FSK4
|
|
*/
|
|
static inline uint8_t
|
|
slice (float s) {
|
|
uint8_t dibit;
|
|
if(s < -2.0) {
|
|
dibit = 3;
|
|
} else if(s < 0.0) {
|
|
dibit = 2;
|
|
} else if(s < 2.0) {
|
|
dibit = 0;
|
|
} else {
|
|
dibit = 1;
|
|
}
|
|
return dibit;
|
|
}
|
|
|
|
int
|
|
repeater_p25_frame_fb::general_work (int noutput_items,
|
|
gr_vector_int &ninput_items,
|
|
gr_vector_const_void_star &input_items,
|
|
gr_vector_void_star &output_items)
|
|
{
|
|
|
|
const float *in = (const float *) input_items[0];
|
|
unsigned char *out = (unsigned char *) output_items[0];
|
|
|
|
for (int i = 0; i < noutput_items; i++){
|
|
rx_sym(slice(in[i]) ^ reverse_p);
|
|
surplus++;
|
|
}
|
|
int amt_produce = 0;
|
|
if (surplus > 0)
|
|
amt_produce = surplus;
|
|
if (amt_produce > noutput_items)
|
|
amt_produce = noutput_items;
|
|
if (amt_produce > (int)symbol_queue.size())
|
|
amt_produce = symbol_queue.size();
|
|
if (amt_produce > 0) {
|
|
copy(symbol_queue.begin(), symbol_queue.begin() + amt_produce, out);
|
|
symbol_queue.erase(symbol_queue.begin(), symbol_queue.begin() + amt_produce);
|
|
surplus -= amt_produce;
|
|
}
|
|
// printf ("work: ninp[0]: %d nout: %d size %d produce: %d surplus %d\n", ninput_items[0], noutput_items, symbol_queue.size(), amt_produce, surplus);
|
|
consume_each(noutput_items);
|
|
// Tell runtime system how many output items we produced.
|
|
return amt_produce;
|
|
}
|