op25
/
op25
3
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

p25p2 pdu updates, new 'sync' plot mode

master
Max 2022-01-04 15:29:01 -05:00
parent 8a948a3bee
commit 5818d58dba
9 changed files with 345 additions and 211 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

131
op25/gr-op25_repeater/apps/gr_gnuplot.py
View File

@ -24,13 +24,15 @@ import os
import time
import subprocess
import json
import threading
import glob
from gnuradio import gr, gru, eng_notation
from gnuradio import blocks, audio
from gnuradio.eng_option import eng_option
import numpy as np
from gnuradio import gr
from math import pi
from math import pi, sin, cos
_def_debug = 0
_def_sps = 10
@ -55,6 +57,14 @@ def limit(a,lim):
return lim
return a
def ensure_str(s): # for python 2/3
if isinstance(s[0], str):
return s
ns = ''
for i in range(len(s)):
ns += chr(s[i])
return ns
PSEQ = 0
class wrap_gp(object):
@ -127,6 +137,9 @@ class wrap_gp(object):
def set_output_dir(self, v):
self.output_dir = v
def set_sps(self, sps):
self.sps = sps
def plot(self, buf, bufsz, mode='eye'):
BUFSZ = bufsz
consumed = min(len(buf), BUFSZ-len(self.buf))
@ -203,6 +216,25 @@ class wrap_gp(object):
s += '%f\n' % (b)
s += 'e\n'
plots.append('"-" with lines')
elif mode == 'sync':
s_abs = np.abs(self.buf)
sums = np.zeros(self.sps)
for i in range(self.sps):
sums[i] = np.sum(s_abs[range(i, len(self.buf), self.sps)])
am = np.argmax(sums)
samples = self.buf[am:]
a1 = -np.angle(samples[0])
rz = cos(a1) + 1j * sin(a1)
while len(samples) >= self.sps+1:
for i in range(self.sps+1):
z = samples[i] * rz
s += '%f\t%f\n' % (z.real, z.imag)
s += 'e\n'
plots.append('"-" with linespoints')
samples = samples[self.sps:]
self.buf = np.array([])
# FFT processing needs to be completed to maintain the weighted average buckets
@ -285,6 +317,12 @@ class wrap_gp(object):
h+= 'set yrange [-4:4]\n'
h+= 'set title "Datascope %s" %s\n' % (self.title, label_color)
plot_color = ''
elif mode == 'sync':
h += 'set object 1 rect from screen 0,0 to screen 1,1 %s behind\n' % (background_color)
h += 'set size square\n'
h += 'set xtics %s\n' % (tic_color)
h += 'set ytics %s\n' % (tic_color)
h += 'set border %s\n' % (border_color)
elif mode == 'symbol':
h+= background
h+= 'set yrange [-4:4]\n'
@ -319,6 +357,8 @@ class wrap_gp(object):
title = self.title
h+= 'set yrange [-1.1:1.1]\n'
h+= 'set title "%s" %s\n' % (title, label_color)
if self.output_dir:
s += 'set output\n' ## flush output png
dat = '%s%splot %s %s\n%s' % (h0, h, ','.join(plots), plot_color, s)
if self.logfile is not None:
with open(self.logfile, 'a') as fd:
@ -328,7 +368,11 @@ class wrap_gp(object):
self.gp.poll()
if self.gp.returncode is None: # make sure gnuplot is still running
try:
self.gp.stdin.write(dat)
rc = self.gp.stdin.write(dat)
except (IOError, ValueError):
pass
try:
self.gp.stdin.flush()
except (IOError, ValueError):
pass
if filename:
@ -486,6 +530,89 @@ class mixer_sink_c(gr.sync_block):
def kill(self):
self.gnuplot.kill()
class sync_plot(threading.Thread):
"""
"""
def __init__(self, debug = _def_debug, block = None, **kwds):
threading.Thread.__init__ (self, **kwds)
self.setDaemon(1)
self.SLEEP_TIME = 3 ## TODO - make more configurable
self.sleep_until = time.time() + self.SLEEP_TIME
self.last_file_time = time.time()
self.keep_running = True
self.debug = debug
self.warned = False
block.enable_sync_plot(True) # block must refer to a gardner/costas instance
self.blk_id = block.unique_id()
self.gnuplot = wrap_gp(sps = _def_sps)
self.start()
def run(self):
while self.keep_running == True:
curr_time = time.time()
if curr_time < self.sleep_until:
time.sleep(1.0)
if self.keep_running == False:
break
else:
self.sleep_until = time.time() + self.SLEEP_TIME
self.check_update()
def read_raw_file(self, fn):
s = open(fn, 'rb').read()
s_msg = ensure_str(s)
p = s_msg.find('\n')
if p < 1 or p > 24:
return None # error
hdrline = s_msg[:p]
rest = s[p+1:]
params = hdrline.split()
params = [int(p) for p in params] #idx, p1p2, sps, error
idx = params[0]
p1p2 = params[1]
sps = params[2]
error_amt = params[3]
self.gnuplot.set_sps(sps)
if error_amt != 0:
self.set_title("Tuning Error %d" % error_amt)
else:
self.set_title("")
samples = np.frombuffer(rest, dtype=np.complex64)
samples2 = np.concatenate((samples[idx:], samples[:idx]))
needed = sps * 25 if p1p2 == 1 else sps * 21
if len(samples2) < needed:
if not self.warned:
self.warned = True
sys.stderr.write('read_raw_file: insufficient samples %d, needed %d\n' % (needed, len(samples2)))
elif len(samples2) > needed:
trim = len(samples2) - needed
samples2 = samples2[trim:]
return samples2 # return trimmed buf in np.complex64 format
def check_update(self):
patt = 'sample-%d*.dat' % (self.blk_id)
names = glob.glob(patt)
if len(names) < 1: # no files to work with
return
d = {n: os.stat(n).st_mtime for n in names}
ds = sorted(d.items(), key=lambda x:x[1], reverse = True)[0]
if ds[1] <= self.last_file_time:
return
self.last_file_time = ds[1]
dat = self.read_raw_file(ds[0])
self.gnuplot.plot(dat, len(dat), mode='sync')
def kill(self):
self.keep_running = False
def set_title(self, title):
self.gnuplot.set_title(title)
def kill(self):
self.gnuplot.kill()
class symbol_sink_f(gr.sync_block):
"""
"""

9
op25/gr-op25_repeater/apps/multi_rx.py
View File

@ -49,6 +49,7 @@ from gr_gnuplot import mixer_sink_c
from gr_gnuplot import symbol_sink_f
from gr_gnuplot import eye_sink_f
from gr_gnuplot import setup_correlation
from gr_gnuplot import sync_plot
from nxdn_trunking import cac_message
@ -311,6 +312,14 @@ class channel(object):
sinks = setup_correlation(sps, self.name, self.demod.connect_bb)
self.kill_sink += sinks
self.sinks += sinks
elif plot == 'sync':
assert config['demod_type'] == 'cqpsk' ## sync plot requires cqpsk demod type
i = len(self.sinks)
sink = sync_plot(block = self.demod.clock)
sink.set_title(self.name)
self.sinks.append(sink)
self.kill_sink.append(self.sinks[i])
# does not issue self.connect()
else:
sys.stderr.write('unrecognized plot type %s\n' % plot)
return

6
op25/gr-op25_repeater/apps/rx.py
View File

@ -66,6 +66,7 @@ from gr_gnuplot import symbol_sink_f
from gr_gnuplot import eye_sink_f
from gr_gnuplot import mixer_sink_c
from gr_gnuplot import setup_correlation
from gr_gnuplot import sync_plot
from terminal import op25_terminal
from sockaudio import audio_thread
@ -328,6 +329,11 @@ class p25_rx_block (gr.top_block):
elif plot_mode == 'correlation':
assert self.options.demod_type == 'fsk4' ## correlation plot requires fsk4 demod type
self.plot_sinks += setup_correlation(sps, "", self.demod.connect_bb)
elif plot_mode == 'sync':
assert self.options.demod_type == 'cqpsk' ## sync plot requires cqpsk demod-type
sink = sync_plot(block = self.demod.clock)
self.plot_sinks.append(sink)
# no flowgraph connection in this plot
else:
raise ValueError('unsupported plot type: %s' % plot_mode)
if self.is_http_term():

84
op25/gr-op25_repeater/apps/trunking.py
View File

@ -524,11 +524,70 @@ class trunked_system (object):
def decode_tdma_blk(self, blk):
self.stats['tsbks'] += 1
op = (blk[0] >> 6) & 3
moc = blk[0] & 0x3f
msg0 = get_ordinals(blk[:1])
op = (msg0 >> 6) & 3
moc = msg0 & 0x3f
if self.debug > 1:
sys.stderr.write('tdma_cc: decode_blk: op %x moc %x\n' % (op, moc))
if op == 1 and moc == 0x3c: # adjacent
sys.stderr.write('tdma_cc: decode_blk: op %x moc %x len %d msg0 0x%x\n' % (op, moc, len(blk), msg0))
if op == 0 and moc == 0: # null msg
if self.debug > 10:
sys.stderr.write('tdma_cc: null block\n')
return -1;
if op == 0 and moc == 8: # null msg zero bias
if self.debug > 10:
sys.stderr.write('tdma_cc: null block zero bias\n')
return -1;
if op == 1 and moc == 0x38: # system service broadcast
if self.debug > 10:
sys.stderr.write('tdma_cc: system service broadcast\n')
msglen = 9
elif op == 3 and moc == 0x16: # SNDCP data ch
if self.debug > 10:
sys.stderr.write('tdma_cc: SNDCP data channel announce\n')
msglen = 9
elif op == 0 and moc == 1: # group voice channel user
msglen = 7
msg = get_ordinals(blk[:msglen])
options = (msg >> 40) & 0xff
ga1 = (msg >> 24) & 0xffff
sa1 = (msg ) & 0xffffff
if self.debug > 1:
sys.stderr.write('decode_blk: group voice channel user opts %x ga %x sa %x\n' % (options, ga1, sa1))
elif op == 0 and moc == 0x30: # power control sq
msglen = 5
if self.debug > 1:
sys.stderr.write('decode_blk: power control sq\n')
elif op == 1 and moc == 0x30: # sync broadcast
msglen = 9
msg = get_ordinals(blk[:msglen])
year = (msg >> 33) & 0x7f
month = (msg >> 29) & 0xf
day = (msg >> 24) & 0x1f
if self.debug > 1:
sys.stderr.write('decode_blk: synchronization broadcast %d %d %d\n' % (year, month, day))
elif op == 1 and moc == 2: # group voice channel grant update
msglen = 9
msg = get_ordinals(blk[:msglen])
ch1 = (msg >> 48) & 0xffff
ga1 = (msg >> 32) & 0xffff
ch2 = (msg >> 16) & 0xffff
ga2 = (msg ) & 0xffff
f1 = self.channel_id_to_frequency(ch1)
f2 = self.channel_id_to_frequency(ch2)
if self.debug > 1:
sys.stderr.write('decode_blk: group voice channel grant update ga1 %d ch1 %s ga2 %d ch2 %s\n' % (ga1, f1, ga2, f2))
opcode = 2 # (p25p1 opcode)
d = {'cc_event': 'grp_v_ch_grant_updt', 'mfrid': 0, 'frequency1': f1, 'group1': self.mk_tg_dict(ga1), 'opcode': opcode, 'tdma_slot': self.get_tdma_slot(ch1) }
self.update_voice_frequency(f1, tgid=ga1, tdma_slot=self.get_tdma_slot(ch1))
if f1 != f2:
self.update_voice_frequency(f2, tgid=ga2, tdma_slot=self.get_tdma_slot(ch2))
d['frequency2'] = f2
d['group2'] = self.mk_tg_dict(ga2)
self.post_event(d)
if self.debug > 10:
sys.stderr.write('phase 2 tsbk02 grant update: chan %s %d %s %d\n' %(self.channel_id_to_string(ch1), ga1, self.channel_id_to_string(ch2), ga2))
elif op == 1 and moc == 0x3c: # adjacent
msglen = 9
msg = get_ordinals(blk[:msglen])
syid = (msg >> 40) & 0xfff
@ -644,12 +703,17 @@ class trunked_system (object):
else:
msglen = -1
if self.debug > 0:
sys.stderr.write ('tdma_cc unknown request: %x %x %02x %02x %02x\n' % (op, moc, blk[0], blk[1], blk[2] ))
sys.stderr.write ('tdma_cc unknown request: %x %x %x\n' % (op, moc, get_ordinals(blk)))
return msglen
def decode_tdma_cc(self, blk):
rc = self.decode_tdma_blk(blk)
# TODO: Attempt to decode remaining half?
while True:
rc = self.decode_tdma_blk(blk)
if rc < 1:
break
if rc >= len(blk):
break
blk = blk[rc:]
def decode_tsbk_harris(self, tsbk, opcode, mfrid):
HARRIS_SGS_EXPIRES = 5.0 # sec.
@ -1361,14 +1425,12 @@ class rx_ctl (object):
# nac is always 1st two bytes
nac = get_ordinals(msgtext[:2])
msgtext = msgtext[2:]
if self.debug > 1:
sys.stderr.write('tdma_cc message nac 0x%x text 0x%x len %d\n' % (nac, get_ordinals(msgtext), len(msgtext)))
if nac not in self.trunked_systems.keys():
sys.stderr.write('tdma_cc received from unexpected NAC 0x%x\n' % nac)
return
tsys = self.trunked_systems[nac]
m1 = msgtext[1]
b1 = (m1 >> 7) & 1
b2 = (m1 >> 6) & 1
moc = m1 & 0x3f
tsys.decode_tdma_cc(msgtext[1:])
return
elif mtype < 0:

1
op25/gr-op25_repeater/include/op25_repeater/gardner_costas_cc.h
View File

@ -54,6 +54,7 @@ namespace gr {
virtual bool is_muted(void) = 0;
virtual void set_tdma(bool) = 0;
virtual bool is_tdma(void) = 0;
virtual void enable_sync_plot(bool) = 0;
};
} // namespace op25_repeater

95
op25/gr-op25_repeater/lib/gardner_costas_cc_impl.cc
View File

@ -35,6 +35,7 @@
#include <stdexcept>
#include <cstdio>
#include <string.h>
#include <sys/time.h>
#include "p25_frame.h"
#include "p25p2_framer.h"
@ -50,6 +51,8 @@ static const int NUM_COMPLEX=100;
static const int FM_COUNT=500; // number of samples per measurement frame
static const int N_FILES = 4; // number of filenames to cycle through
namespace gr {
namespace op25_repeater {
@ -66,6 +69,49 @@ static inline std::complex<float> sgn(std::complex<float>c) {
d_event_type = c; \
}
static inline bool is_future(struct timeval*t) {
struct timeval current_t;
gettimeofday(&current_t,0);
if (t->tv_sec > current_t.tv_sec)
return true;
else if (t->tv_sec < current_t.tv_sec)
return false;
else if (t->tv_usec > current_t.tv_usec)
return true;
else
return false;
}
void gardner_costas_cc_impl::dump_samples(int error_amt) {
// TODO = disk I/O from inside a gr flow graph block work function (tsk tsk)
char tmp_filename[256];
char filename[256];
char line[64];
FILE *fp1;
if (!d_enable_sync_plot)
return;
if (d_prev_sample == NULL)
return;
if (is_future(&d_next_sample_time))
return;
gettimeofday(&d_next_sample_time,0);
d_next_sample_time.tv_sec += 5;
sprintf(filename, "sample-%ld-%d.dat", unique_id(), d_sample_file_id);
d_sample_file_id ++;
d_sample_file_id = d_sample_file_id % N_FILES;
sprintf(line, "%u %d %d %d\n", d_prev_sample_p, (d_is_tdma) ? 2 : 1, (int) (d_omega + 0.5), error_amt);
strcpy(tmp_filename, filename);
strcat(tmp_filename, ".tmp");
fp1 = fopen(tmp_filename, "wb");
if (!fp1)
return;
fwrite (line, 1, strlen(line), fp1);
fwrite (d_prev_sample, 1, d_n_prev_sample * sizeof(gr_complex), fp1);
fclose(fp1);
rename(tmp_filename, filename);
}
uint8_t gardner_costas_cc_impl::slicer(float sym) {
uint8_t dibit = 0;
static const float PI_4 = M_PI / 4.0;
@ -89,36 +135,44 @@ uint8_t gardner_costas_cc_impl::slicer(float sym) {
if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ P25_FRAME_SYNC_MAGIC, 0, 48)) {
// fprintf(stderr, "P25P1 Framing detect\n");
UPDATE_COUNT(' ')
dump_samples(0);
}
else if(check_frame_sync((nid_accum & P25P2_FRAME_SYNC_MASK) ^ P25P2_FRAME_SYNC_MAGIC, 0, 40)) {
// fprintf(stderr, "P25P2 Framing detect\n");
UPDATE_COUNT(' ')
dump_samples(0);
}
if (d_is_tdma) {
if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0x000104015155LL, 0, 40)) {
fprintf(stderr, "TDMA: channel %d tuning error -1200\n", -1);
UPDATE_COUNT('-')
dump_samples(-1200);
}
else if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0xfefbfeaeaaLL, 0, 40)) {
fprintf(stderr, "TDMA: channel %d tuning error +1200\n", -1);
UPDATE_COUNT('+')
dump_samples(1200);
}
else if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0xa8a2a80800LL, 0, 40)) {
fprintf(stderr, "TDMA: channel %d tuning error +/- 2400\n", -1);
UPDATE_COUNT('|')
dump_samples(2400);
}
} else {
if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0x001050551155LL, 0, 48)) {
// fprintf(stderr, "tuning error -1200\n");
UPDATE_COUNT('-')
dump_samples(-1200);
}
else if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0xFFEFAFAAEEAALL, 0, 48)) {
// fprintf(stderr, "tuning error +1200\n");
UPDATE_COUNT('+')
dump_samples(1200);
}
else if(check_frame_sync((nid_accum & P25_FRAME_SYNC_MASK) ^ 0xAA8A0A008800LL, 0, 48)) {
// fprintf(stderr, "tuning error +/- 2400\n");
UPDATE_COUNT('|')
dump_samples(2400);
}
}
if (d_event_type == ' ' || d_event_count < 5) {
@ -165,11 +219,15 @@ uint8_t gardner_costas_cc_impl::slicer(float sym) {
d_event_count(0), d_event_type(' '),
d_symbol_seq(samples_per_symbol * 4800),
d_update_request(0),
d_fm(0), d_fm_accum(0), d_fm_count(0), d_muted(false), d_is_tdma(false)
d_fm(0), d_fm_accum(0), d_fm_count(0), d_muted(false), d_is_tdma(false),
d_enable_sync_plot(false),
d_prev_sample(NULL), d_n_prev_sample(0), d_prev_sample_p(0),
d_sample_file_id(0)
{
set_omega(samples_per_symbol);
set_relative_rate (1.0 / d_omega);
set_history(d_twice_sps); // ensure extra input is available
gettimeofday(&d_next_sample_time, 0);
}
/*
@ -177,6 +235,10 @@ uint8_t gardner_costas_cc_impl::slicer(float sym) {
*/
gardner_costas_cc_impl::~gardner_costas_cc_impl()
{
if (d_prev_sample != NULL) {
free(d_prev_sample);
d_prev_sample = NULL;
}
delete [] d_dl;
delete d_interp;
}
@ -245,21 +307,20 @@ gardner_costas_cc_impl::phase_error_tracking(gr_complex sample)
phase_error = phase_error_detector_qpsk(sample);
d_freq += d_beta*phase_error*abs(sample); // adjust frequency based on error
d_phase += d_freq + d_alpha*phase_error*abs(sample); // adjust phase based on error
d_phase += d_alpha*phase_error*abs(sample); // adjust phase based on error
// Make sure we stay within +-2pi
while(d_phase > M_TWOPI)
// Make sure we stay within +-pi
while(d_phase > M_PI)
d_phase -= M_TWOPI;
while(d_phase < -M_TWOPI)
while(d_phase < -M_PI)
d_phase += M_TWOPI;
// Limit the frequency range
d_freq = gr::branchless_clip(d_freq, d_max_freq);
#if VERBOSE_COSTAS
printf("cl: phase_error: %f phase: %f freq: %f sample: %f+j%f constellation: %f+j%f\n",
phase_error, d_phase, d_freq, sample.real(), sample.imag(),
d_constellation[d_current_const_point].real(), d_constellation[d_current_const_point].imag());
fprintf(stderr, "COSTAS\t%f\t%f\t%f\t%f+j%f\n",
phase_error, d_phase, d_freq, sample.real(), sample.imag());
#endif
}
@ -277,21 +338,33 @@ gardner_costas_cc_impl::general_work (int noutput_items,
gr_complex interp_samp, interp_samp_mid, diffdec;
float error_real, error_imag, symbol_error;
if (d_enable_sync_plot && d_prev_sample == NULL) {
d_n_prev_sample = (int) (d_omega + 0.5); // sps
d_n_prev_sample *= (d_is_tdma) ? 32 : 25; // enough for p25p1 or p25p2 sync
d_prev_sample = (gr_complex *) calloc(d_n_prev_sample, sizeof(gr_complex));
}
while((o < noutput_items) && (i < ninput_items[0])) {
while((d_mu > 1.0) && (i < ninput_items[0])) {
d_mu --;
d_phase += d_freq;
// Keep phase clamped and not walk to infinity
while(d_phase > M_TWOPI)
while(d_phase > M_PI)
d_phase -= M_TWOPI;
while(d_phase < -M_TWOPI)
while(d_phase < -M_PI)
d_phase += M_TWOPI;
nco = gr_expj(d_phase+d_theta); // get the NCO value for derotating the curr
symbol = in[i];
sample = nco*symbol; // get the downconverted symbol
if (d_enable_sync_plot && d_prev_sample != NULL) {
d_prev_sample[d_prev_sample_p] = sample;
d_prev_sample_p ++;
d_prev_sample_p %= d_n_prev_sample;
}
d_dl[d_dl_index] = sample;
d_dl[d_dl_index + d_twice_sps] = sample;
d_dl_index ++;
@ -345,7 +418,7 @@ gardner_costas_cc_impl::general_work (int noutput_items,
d_omega = d_omega + d_gain_omega * symbol_error * abs(interp_samp); // update omega based on loop error
d_omega = d_omega_mid + gr::branchless_clip(d_omega-d_omega_mid, d_omega_rel); // make sure we don't walk away
#if VERBOSE_GARDNER
printf("%f\t%f\t%f\t%f\t%f\n", symbol_error, d_mu, d_omega, error_real, error_imag);
fprintf(stderr, "%f\t%f\t%f\t%f\t%f\n", symbol_error, d_mu, d_omega, error_real, error_imag);
#endif
} else {
symbol_error = 0;

10
op25/gr-op25_repeater/lib/gardner_costas_cc_impl.h
View File

@ -78,6 +78,7 @@ namespace gr {
inline bool is_muted(void) { return d_muted; }
inline void set_tdma(bool v) { d_is_tdma = v; }
inline bool is_tdma(void) { return d_is_tdma; }
inline void enable_sync_plot(bool v) { d_enable_sync_plot = v; }
protected:
bool input_sample0(gr_complex, gr_complex& outp);
@ -121,9 +122,18 @@ protected:
int d_fm_count;
bool d_muted;
bool d_is_tdma;
bool d_enable_sync_plot;
gr_complex *d_prev_sample;
unsigned int d_n_prev_sample;
unsigned int d_prev_sample_p;
struct timeval d_next_sample_time;
int d_sample_file_id;
float phase_error_detector_qpsk(gr_complex sample);
void phase_error_tracking(gr_complex sample);
void dump_samples(int);
};
} // namespace op25_repeater

219
op25/gr-op25_repeater/lib/p25p2_tdma.cc
View File

@ -1,4 +1,4 @@
// P25 TDMA Decoder (C) Copyright 2013, 2014 Max H. Parke KA1RBI
// P25 TDMA Decoder (C) Copyright 2013, 2014, 2021 Max H. Parke KA1RBI
// Copyright 2017 Graham J. Norbury (modularization rewrite)
//
// This file is part of OP25
@ -38,6 +38,7 @@
static const int BURST_SIZE = 180;
static const int SUPERFRAME_SIZE = (12*BURST_SIZE);
static const int which_slot[] = {0,1,0,1,0,1,0,1,0,1,1,0};
static uint16_t crc12(const uint8_t bits[], unsigned int len) {
uint16_t crc=0;
@ -70,24 +71,6 @@ static bool crc12_ok(const uint8_t bits[], unsigned int len) {
return (crc == crc12(bits,len));
}
static const uint8_t mac_msg_len[256] = {
0, 7, 8, 7, 0, 16, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 14, 15, 0, 0, 15, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
5, 7, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
9, 7, 9, 0, 9, 8, 9, 0, 0, 0, 9, 0, 0, 0, 0, 0,
0, 0, 0, 0, 9, 7, 0, 0, 0, 0, 7, 0, 0, 8, 14, 7,
9, 9, 0, 0, 9, 0, 0, 9, 0, 0, 7, 0, 0, 7, 0, 0,
0, 0, 0, 9, 9, 9, 0, 0, 9, 9, 9, 11, 9, 9, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
11, 0, 0, 8, 15, 12, 15, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 9, 0, 0, 0, 11, 0, 0, 0, 0, 11,
0, 0, 0, 0, 0, 0, 0, 0, 0, 8, 11, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 11, 13, 11, 0, 0, 0 };
p25p2_tdma::p25p2_tdma(const op25_audio& udp, int slotid, int debug, bool do_msgq, gr::msg_queue::sptr queue, std::deque<int16_t> &qptr, bool do_audio_output, int msgq_id) : // constructor
op25audio(udp),
write_bufp(0),
@ -122,6 +105,7 @@ bool p25p2_tdma::rx_sym(uint8_t sym)
void p25p2_tdma::set_slotid(int slotid)
{
memset(tdma_xormask, 0, SUPERFRAME_SIZE);
assert (slotid == 0 || slotid == 1);
d_slotid = slotid;
}
@ -141,15 +125,33 @@ int p25p2_tdma::process_mac_pdu(const uint8_t byte_buf[], const unsigned int len
{
unsigned int opcode = (byte_buf[0] >> 5) & 0x7;
unsigned int offset = (byte_buf[0] >> 2) & 0x7;
bool my_slot = (which_slot[sync.tdma_slotid()] == d_slotid);
if (d_debug >= 10) {
fprintf(stderr, "%s process_mac_pdu: opcode %d len %d\n", logts.get(), opcode, len);
fprintf(stderr, "%s process_mac_pdu: opcode %d len %d buf %02x %02x %02x\n", logts.get(), opcode, len, byte_buf[0], byte_buf[1], byte_buf[2]);
}
if (opcode == 2) { // MAC_END_PTT
uint16_t colorcd = ((byte_buf[1] & 0x0f) << 8) + byte_buf[2];
if (colorcd != d_nac && d_debug > 0)
fprintf(stderr, "p25p2_tdma_ process_mac_pdu: MAC_END_PTT color code 0x%x does not match d_nac 0x%x channel %d\n", colorcd, d_nac, d_msgq_id);
}
if (opcode == 3 || opcode == 4 || opcode == 6) { // send msg for MAC_IDLE, MAC_ACTIVE, MAC_HANGTIME
char nac_color[2];
nac_color[0] = d_nac >> 8;
nac_color[1] = d_nac & 0xff;
send_msg(std::string(nac_color, 2) + std::string((const char *)byte_buf, len), -6);
}
if (opcode != 0 && !my_slot) // for all except MAC_SIGNAL, ignore if on oppo. slot
return -1;
switch (opcode)
{
case 0: // MAC_SIGNAL
handle_mac_signal(byte_buf, len);
return -1;
break;
case 1: // MAC_PTT
@ -161,20 +163,22 @@ int p25p2_tdma::process_mac_pdu(const uint8_t byte_buf[], const unsigned int len
break;
case 3: // MAC_IDLE
handle_mac_idle(byte_buf, len);
op25audio.send_audio_flag(op25_audio::DRAIN);
break;
case 4: // MAC_ACTIVE
handle_mac_active(byte_buf, len);
break;
case 6: // MAC_HANGTIME
handle_mac_hangtime(byte_buf, len);
op25audio.send_audio_flag(op25_audio::DRAIN);
break;
default:
if (d_debug > 0)
fprintf(stderr, "p25p2_tdma_ process_mac_pdu: unrecognized opcode 0x%x channel %d\n", opcode, d_msgq_id);
break;
}
// maps sacch opcodes into phase I duid values
static const int opcode_map[8] = {3, 5, 15, 15, 5, 3, 3, 3};
static const int opcode_map[8] = {7, 5, 15, 15, 5, 3, 3, 3};
return opcode_map[opcode];
}
@ -233,162 +237,6 @@ void p25p2_tdma::handle_mac_end_ptt(const uint8_t byte_buf[], const unsigned int
op25audio.send_audio_flag(op25_audio::DRAIN);
}
void p25p2_tdma::handle_mac_idle(const uint8_t byte_buf[], const unsigned int len)
{
if (d_debug >= 10)
fprintf(stderr, "%s MAC_IDLE: ", logts.get());
decode_mac_msg(byte_buf, len);
op25audio.send_audio_flag(op25_audio::DRAIN);
if (d_debug >= 10)
fprintf(stderr, "\n");
}
void p25p2_tdma::handle_mac_active(const uint8_t byte_buf[], const unsigned int len)
{
if (d_debug >= 10)
fprintf(stderr, "%s MAC_ACTIVE: ", logts.get());
decode_mac_msg(byte_buf, len);
if (d_debug >= 10)
fprintf(stderr, "\n");
}
void p25p2_tdma::handle_mac_hangtime(const uint8_t byte_buf[], const unsigned int len)
{
if (d_debug >= 10)
fprintf(stderr, "%s MAC_HANGTIME: ", logts.get());
decode_mac_msg(byte_buf, len);
op25audio.send_audio_flag(op25_audio::DRAIN);
if (d_debug >= 10)
fprintf(stderr, "\n");
}
void p25p2_tdma::decode_mac_msg(const uint8_t byte_buf[], const unsigned int len)
{
std::string s;
uint8_t b1b2, cfva, mco, lra, rfss, site_id, ssc, svcopts[3], msg_ptr, msg_len;
uint16_t chan[3], ch_t[2], ch_r[2], colorcd, grpaddr[3], sys_id;
uint32_t srcaddr, wacn_id;
for (msg_ptr = 1; msg_ptr < len; )
{
b1b2 = byte_buf[msg_ptr] >> 6;
mco = byte_buf[msg_ptr] & 0x3f;
msg_len = mac_msg_len[(b1b2 << 6) + mco];
if (d_debug >= 10)
fprintf(stderr, "mco=%01x/%02x", b1b2, mco);
switch(byte_buf[msg_ptr])
{
case 0x00: // Null message
break;
case 0x40: // Group Voice Channel Grant Abbreviated
svcopts[0] = (byte_buf[msg_ptr+1] ) ;
chan[0] = (byte_buf[msg_ptr+2] << 8) + byte_buf[msg_ptr+3];
grpaddr[0] = (byte_buf[msg_ptr+4] << 8) + byte_buf[msg_ptr+5];
srcaddr = (byte_buf[msg_ptr+6] << 16) + (byte_buf[msg_ptr+7] << 8) + byte_buf[msg_ptr+8];
if (d_debug >= 10)
fprintf(stderr, ", svcopts=0x%02x, ch=%u, grpaddr=%u, srcaddr=%u", svcopts[0], chan[0], grpaddr[0], srcaddr);
break;
case 0xc0: // Group Voice Channel Grant Extended
svcopts[0] = (byte_buf[msg_ptr+1] ) ;
ch_t[0] = (byte_buf[msg_ptr+2] << 8) + byte_buf[msg_ptr+3];
ch_r[0] = (byte_buf[msg_ptr+4] << 8) + byte_buf[msg_ptr+5];
grpaddr[0] = (byte_buf[msg_ptr+6] << 8) + byte_buf[msg_ptr+7];
srcaddr = (byte_buf[msg_ptr+8] << 16) + (byte_buf[msg_ptr+9] << 8) + byte_buf[msg_ptr+10];
if (d_debug >= 10)
fprintf(stderr, ", svcopts=0x%02x, ch_t=%u, ch_t=%u, grpaddr=%u, srcaddr=%u", svcopts[0], ch_t[0], ch_r[0], grpaddr[0], srcaddr);
break;
case 0x01: // Group Voice Channel User Message Abbreviated
grpaddr[0] = (byte_buf[msg_ptr+2] << 8) + byte_buf[msg_ptr+3];
srcaddr = (byte_buf[msg_ptr+4] << 16) + (byte_buf[msg_ptr+5] << 8) + byte_buf[msg_ptr+6];
if (d_debug >= 10)
fprintf(stderr, ", grpaddr=%u, srcaddr=%u", grpaddr[0], srcaddr);
s = "{\"srcaddr\" : " + std::to_string(srcaddr) + ", \"grpaddr\": " + std::to_string(grpaddr[0]) + ", \"nac\" : " + std::to_string(d_nac) + "}";
send_msg(s, -3);
break;
case 0x42: // Group Voice Channel Grant Update
chan[0] = (byte_buf[msg_ptr+1] << 8) + byte_buf[msg_ptr+2];
grpaddr[0] = (byte_buf[msg_ptr+3] << 8) + byte_buf[msg_ptr+4];
chan[1] = (byte_buf[msg_ptr+5] << 8) + byte_buf[msg_ptr+6];
grpaddr[1] = (byte_buf[msg_ptr+7] << 8) + byte_buf[msg_ptr+8];
if (d_debug >= 10)
fprintf(stderr, ", ch_1=%u, grpaddr1=%u, ch_2=%u, grpaddr2=%u", chan[0], grpaddr[0], chan[1], grpaddr[1]);
break;
case 0xc3: // Group Voice Channel Grant Update Explicit
svcopts[0] = (byte_buf[msg_ptr+1] ) ;
ch_t[0] = (byte_buf[msg_ptr+2] << 8) + byte_buf[msg_ptr+3];
ch_r[0] = (byte_buf[msg_ptr+4] << 8) + byte_buf[msg_ptr+5];
grpaddr[0] = (byte_buf[msg_ptr+6] << 8) + byte_buf[msg_ptr+7];
if (d_debug >= 10)
fprintf(stderr, ", svcopts=0x%02x, ch_t=%u, ch_r=%u, grpaddr=%u", svcopts[0], ch_t[0], ch_r[0], grpaddr[0]);
break;
case 0x05: // Group Voice Channel Grant Update Multiple
svcopts[0] = (byte_buf[msg_ptr+ 1] ) ;
chan[0] = (byte_buf[msg_ptr+ 2] << 8) + byte_buf[msg_ptr+ 3];
grpaddr[0] = (byte_buf[msg_ptr+ 4] << 8) + byte_buf[msg_ptr+ 5];
svcopts[1] = (byte_buf[msg_ptr+ 6] ) ;
chan[1] = (byte_buf[msg_ptr+ 7] << 8) + byte_buf[msg_ptr+ 8];
grpaddr[1] = (byte_buf[msg_ptr+ 9] << 8) + byte_buf[msg_ptr+10];
svcopts[2] = (byte_buf[msg_ptr+11] ) ;
chan[2] = (byte_buf[msg_ptr+12] << 8) + byte_buf[msg_ptr+13];
grpaddr[2] = (byte_buf[msg_ptr+14] << 8) + byte_buf[msg_ptr+15];
if (d_debug >= 10)
fprintf(stderr, ", svcopt1=0x%02x, ch_1=%u, grpaddr1=%u, svcopt2=0x%02x, ch_2=%u, grpaddr2=%u, svcopt3=0x%02x, ch_3=%u, grpaddr3=%u", svcopts[0], chan[0], grpaddr[0], svcopts[1], chan[1], grpaddr[1], svcopts[2], chan[2], grpaddr[2]);
break;
case 0x25: // Group Voice Channel Grant Update Multiple Explicit
svcopts[0] = (byte_buf[msg_ptr+ 1] ) ;
ch_t[0] = (byte_buf[msg_ptr+ 2] << 8) + byte_buf[msg_ptr+ 3];
ch_r[0] = (byte_buf[msg_ptr+ 4] << 8) + byte_buf[msg_ptr+ 5];
grpaddr[0] = (byte_buf[msg_ptr+ 6] << 8) + byte_buf[msg_ptr+ 7];
svcopts[1] = (byte_buf[msg_ptr+ 8] ) ;
ch_t[1] = (byte_buf[msg_ptr+ 9] << 8) + byte_buf[msg_ptr+10];
ch_r[1] = (byte_buf[msg_ptr+11] << 8) + byte_buf[msg_ptr+12];
grpaddr[1] = (byte_buf[msg_ptr+13] << 8) + byte_buf[msg_ptr+14];
if (d_debug >= 10)
fprintf(stderr, ", svcopt1=0x%02x, ch_t1=%u, ch_r1=%u, grpaddr1=%u, svcopt2=0x%02x, ch_t2=%u, ch_r2=%u, grpaddr2=%u", svcopts[0], ch_t[0], ch_r[0], grpaddr[0], svcopts[1], ch_t[1], ch_r[1], grpaddr[1]);
break;
case 0x7b: // Network Status Broadcast Abbreviated
lra = byte_buf[msg_ptr+1];
wacn_id = (byte_buf[msg_ptr+2] << 12) + (byte_buf[msg_ptr+3] << 4) + (byte_buf[msg_ptr+4] >> 4);
sys_id = ((byte_buf[msg_ptr+4] & 0x0f) << 8) + byte_buf[msg_ptr+5];
chan[0] = (byte_buf[msg_ptr+6] << 8) + byte_buf[msg_ptr+7];
ssc = byte_buf[msg_ptr+8];
colorcd = ((byte_buf[msg_ptr+9] & 0x0f) << 8) + byte_buf[msg_ptr+10];
if (d_debug >= 10)
fprintf(stderr, ", lra=0x%02x, wacn_id=0x%05x, sys_id=0x%03x, ch=%u, ssc=0x%02x, colorcd=%03x", lra, wacn_id, sys_id, chan[0], ssc, colorcd);
break;
case 0x7c: // Adjacent Status Broadcast Abbreviated
lra = byte_buf[msg_ptr+1];
cfva = (byte_buf[msg_ptr+2] >> 4);
sys_id = ((byte_buf[msg_ptr+2] & 0x0f) << 8) + byte_buf[msg_ptr+3];
rfss = byte_buf[msg_ptr+4];
site_id = byte_buf[msg_ptr+5];
chan[0] = (byte_buf[msg_ptr+6] << 8) + byte_buf[msg_ptr+7];
ssc = byte_buf[msg_ptr+8];
if (d_debug >= 10)
fprintf(stderr, ", lra=0x%02x, cfva=0x%01x, sys_id=0x%03x, rfss=%u, site=%u, ch=%u, ssc=0x%02x", lra, cfva, sys_id, rfss, site_id, chan[0], ssc);
break;
case 0xfc: // Adjacent Status Broadcast Extended
break;
case 0xfb: // Network Status Broadcast Extended
colorcd = ((byte_buf[msg_ptr+11] & 0x0f) << 8) + byte_buf[msg_ptr+12];
if (d_debug >= 10)
fprintf(stderr, ", colorcd=%03x", colorcd);
break;
}
msg_ptr = (msg_len == 0) ? len : (msg_ptr + msg_len); // TODO: handle variable length messages
if ((d_debug >= 10) && (msg_ptr < len))
fprintf(stderr,", ");
}
}
int p25p2_tdma::handle_acch_frame(const uint8_t dibits[], bool fast, bool is_lcch)
{
int i, j, rc;
@ -472,15 +320,14 @@ int p25p2_tdma::handle_acch_frame(const uint8_t dibits[], bool fast, bool is_lcc
bool crc_ok = (is_lcch) ? (crc16(bits, len) == 0) : crc12_ok(bits, len);
int olen = (is_lcch) ? 23 : len/8;
if (d_debug >= 1)
fprintf(stderr, "p25p2_tdma: crc%d result: %s, length %d\n", (is_lcch) ? 16 : 12, (crc_ok) ? "ok" : "failed", olen);
rc = -1;
if (crc_ok) { // TODO: rewrite crc12 so we don't have to do so much bit manipulation
// fprintf(stderr, "crc12 ok\n");
for (int i=0; i<olen; i++) {
byte_buf[i] = (bits[i*8 + 0] << 7) + (bits[i*8 + 1] << 6) + (bits[i*8 + 2] << 5) + (bits[i*8 + 3] << 4) + (bits[i*8 + 4] << 3) + (bits[i*8 + 5] << 2) + (bits[i*8 + 6] << 1) + (bits[i*8 + 7] << 0);
}
rc = process_mac_pdu(byte_buf, olen);
} else {
// fprintf(stderr, "crc12 failed\n");
}
return rc;
}
@ -537,16 +384,14 @@ int p25p2_tdma::handle_frame(void)
int p25p2_tdma::handle_packet(const uint8_t dibits[])
{
int rc = -1;
static const int which_slot[] = {0,1,0,1,0,1,0,1,0,1,1,0};
packets++;
sync.check_confidence(dibits);
if (!sync.in_sync())
return -1;
const uint8_t* burstp = &dibits[10];
uint8_t xored_burst[BURST_SIZE - 10];
bool my_slot = (which_slot[sync.tdma_slotid()] == d_slotid);
int burst_type = duid.duid_lookup(duid.extract_duid(burstp));
if (which_slot[sync.tdma_slotid()] != d_slotid && burst_type != 13) // ignore if on oppo. slot and not CC
return -1;
for (int i=0; i<BURST_SIZE - 10; i++) {
xored_burst[i] = burstp[i] ^ tdma_xormask[sync.tdma_slotid() * BURST_SIZE + i];
}
@ -554,6 +399,8 @@ int p25p2_tdma::handle_packet(const uint8_t dibits[])
fprintf(stderr, "%s TDMA burst type=%d\n", logts.get(), burst_type);
}
if (burst_type == 0 || burst_type == 6) { // 4V or 2V burst
if (!my_slot) // ignore if on oppo. slot
return -1;
track_vb(burst_type);
handle_4V2V_ess(&xored_burst[84]);
if ( !encrypted() ) {

1
op25/gr-op25_repeater/lib/p25p2_tdma.h
View File

@ -96,7 +96,6 @@ private:
void handle_mac_idle(const uint8_t byte_buf[], const unsigned int len) ;
void handle_mac_active(const uint8_t byte_buf[], const unsigned int len) ;
void handle_mac_hangtime(const uint8_t byte_buf[], const unsigned int len) ;
void decode_mac_msg(const uint8_t byte_buf[], const unsigned int len) ;
void handle_4V2V_ess(const uint8_t dibits[]);
inline bool encrypted() { return (ess_algid != 0x80); }