From 4dead3467567237686cf8c240744d6d5709b55ae Mon Sep 17 00:00:00 2001
From: Max <ikj1234i@yahoo.com>
Date: Mon, 18 Aug 2014 12:52:04 -0400
Subject: [PATCH] phase 2 updates

---
 op25/gr-op25_repeater/apps/scope.py           | 92 +++++++++++++------
 op25/gr-op25_repeater/apps/tdma/duid.py       | 47 +++++-----
 op25/gr-op25_repeater/apps/tdma/isch.py       | 46 +++++-----
 .../gr-op25_repeater/apps/tdma/tdma-decode.py | 29 +++---
 op25/gr-op25_repeater/apps/tdma/tdma_audio.py |  4 +-
 op25/gr-op25_repeater/apps/trunking.py        | 12 ++-
 .../include/op25_repeater/gardner_costas_cc.h |  1 +
 .../lib/gardner_costas_cc_impl.cc             | 18 ++--
 op25/gr-op25_repeater/lib/p25p2_frame_impl.cc |  3 +-
 op25/gr-op25_repeater/lib/p25p2_frame_impl.h  |  1 +
 10 files changed, 156 insertions(+), 97 deletions(-)

diff --git a/op25/gr-op25_repeater/apps/scope.py b/op25/gr-op25_repeater/apps/scope.py
index 271b25d..fc6ca76 100755
--- a/op25/gr-op25_repeater/apps/scope.py
+++ b/op25/gr-op25_repeater/apps/scope.py
@@ -58,6 +58,9 @@ import gnuradio.wxgui.plot as plot
 
 import trunking
 
+sys.path.append('tdma')
+import lfsr
+
 #speeds = [300, 600, 900, 1200, 1440, 1800, 1920, 2400, 2880, 3200, 3600, 3840, 4000, 4800, 6000, 6400, 7200, 8000, 9600, 14400, 19200]
 speeds = [4800, 6000]
 
@@ -120,6 +123,7 @@ class p25_rx_block (stdgui2.std_top_block):
         parser.add_option("-N", "--gains", type="string", default=None, help="gain settings")
         parser.add_option("-O", "--audio-output", type="string", default="plughw:0,0", help="audio output device name")
         parser.add_option("-q", "--freq-corr", type="eng_float", default=0.0, help="frequency correction")
+        parser.add_option("-2", "--phase2-tdma", action="store_true", default=False, help="enable phase2 tdma decode")
         (options, args) = parser.parse_args()
         if len(args) != 0:
             parser.print_help()
@@ -261,18 +265,33 @@ class p25_rx_block (stdgui2.std_top_block):
             udp_port = WIRESHARK_PORT
         if self.options.raw_symbols:
             self.sink_sf = blocks.file_sink(gr.sizeof_char, self.options.raw_symbols)
+            self.connect(self.slicer, self.sink_sf)
         do_imbe = 1
         do_output = 1
         do_msgq = 1
         self.sink_s = op25_repeater.p25_frame_assembler(self.options.wireshark_host, udp_port, self.options.verbosity, do_imbe, do_output, do_msgq, self.rx_q)
-        if self.options.vocoder:
-            self.sink_imbe = op25_repeater.vocoder(0, 0, 0, '', 0, 0)
+        if self.options.vocoder and self.options.phase2_tdma:
+            print 'simultaneous phase I and II not supported in this version'
+            sys.exit(1)
+        if self.options.vocoder or self.options.phase2_tdma:
             self.audio_s2f = blocks.short_to_float()
             self.audio_scaler = blocks.multiply_const_ff(1 / 32768.0)
             self.audio_output = audio.sink(8000, self.options.audio_output, True)
-            self.connect(self.sink_imbe, self.audio_s2f, self.audio_scaler, self.audio_output)
+            self.connect(self.audio_s2f, self.audio_scaler, self.audio_output)
+        if self.options.vocoder:
+            self.sink_imbe = op25_repeater.vocoder(0, 0, 0, '', 0, 0)
+            self.connect(self.sink_imbe, self.audio_s2f)
         else:
             self.sink_imbe = blocks.null_sink(gr.sizeof_char)
+        if self.options.phase2_tdma:
+            slotid = 0
+            do_msgq = True
+            debug = self.options.verbosity
+            self.p2decoder = op25_repeater.p25p2_frame(debug, slotid, do_msgq, self.rx_q)
+            self.connect(self.slicer, self.p2decoder, self.audio_s2f)
+        self.tdma_state = False
+        self.xor_cache = {}
+        self.connect(self.buffer, self.slicer, self.sink_s, self.sink_imbe)
         if self.baseband_input:
             gain = self.options.gain
         else:
@@ -309,7 +328,7 @@ class p25_rx_block (stdgui2.std_top_block):
             self.set_channel_offset(0.0, 0, "")
             # local osc
             self.lo_freq = self.options.offset
-            if self.options.audio_if or self.options.ifile:
+            if self.options.audio_if or self.options.ifile or self.options.input:
                 self.lo_freq += self.options.calibration
             self.lo = analog.sig_source_c (channel_rate, analog.GR_SIN_WAVE, self.lo_freq, 1.0, 0)
             self.mixer = blocks.multiply_cc()
@@ -378,6 +397,7 @@ class p25_rx_block (stdgui2.std_top_block):
             # connect it all up
             self.__connect([[source, (self.mixer, 0)],
                             [self.lo, (self.mixer, 1)]])
+            self.connect(self.mixer, self.lpf, self.arb_resampler)
             self.set_connection(fft=1)
             self.connect_demods()
 
@@ -706,6 +726,30 @@ class p25_rx_block (stdgui2.std_top_block):
 
         vbox.Add(hbox, 0, 0)
 
+    def configure_tdma(self, params):
+        if params['tdma'] is not None and not self.options.phase2_tdma:
+            print '***TDMA request for frequency %d failed- phase2_tdma option not enabled' % params['freq']
+            return
+        set_tdma = False
+        if params['tdma'] is not None:
+            set_tdma = True
+        if set_tdma == self.tdma_state:
+            return	# already in desired state
+        self.tdma_state = set_tdma
+        if set_tdma:
+            self.p2decoder.set_slotid(params['tdma'])
+            hash = '%x%x%x' % (params['nac'], params['sysid'], params['wacn'])
+            if hash not in self.xor_cache:
+                sreg = lfsr.p25p2_lfsr(params['nac'], params['sysid'], params['wacn'])
+                self.xor_cache[hash] = ''
+                for c in sreg.xorsyms:
+                    self.xor_cache[hash] += chr(c)
+            self.p2decoder.set_xormask(self.xor_cache[hash])
+            sps = self.basic_rate / 6000
+        else:
+            sps = self.basic_rate / 4800
+        self.clock.set_omega(float(sps))
+
     def change_freq(self, params):
         freq = params['freq']
         offset = params['offset']
@@ -727,6 +771,8 @@ class p25_rx_block (stdgui2.std_top_block):
         else:
             self.set_freq(freq + offset)
 
+        self.configure_tdma(params)
+
         # send msg as event to avoid thread safety problems updating form
         evt = msg_DataEvent(params)
         wx.PostEvent(self.frame, evt)
@@ -800,13 +846,16 @@ class p25_rx_block (stdgui2.std_top_block):
 
     def demod_type_chg(self, val):
         if val == 'FSK4':
-            self.fsk4_demod_mode = True
+            new_demod_mode = True
         else:
-            self.fsk4_demod_mode = False
+            new_demod_mode = False
+        if self.fsk4_demod_mode == new_demod_mode:
+            return
+        self.fsk4_demod_mode = new_demod_mode
         self.lock()
         self.disconnect_demods()
         notebook_sel = self.notebook.GetSelection()
-        if notebook_sel == 4:	# demod symbols
+        if notebook_sel == 0 or notebook_sel == 4:	# spectrum or demod symbols
              self.connect_demods()
         elif notebook_sel == 1 or notebook_sel == 2 or notebook_sel == 5:
              self.connect_fsk4_demod()
@@ -830,8 +879,9 @@ class p25_rx_block (stdgui2.std_top_block):
     #
     def __set_rx_from_file(self, filename, capture_rate):
         file = blocks.file_source(gr.sizeof_gr_complex, filename, True)
-        throttle = gr.throttle(gr.sizeof_gr_complex, capture_rate)
-        self.__connect([[file, throttle]])
+        gain = blocks.multiply_const_cc(self.options.gain)
+        throttle = blocks.throttle(gr.sizeof_gr_complex, capture_rate)
+        self.__connect([[file, gain, throttle]])
         self.__build_graph(throttle, capture_rate)
 
     # setup to rx from Audio
@@ -1058,8 +1108,7 @@ class p25_rx_block (stdgui2.std_top_block):
     #
     def open_file(self, capture_file):
         try:
-            self.__read_file_properties(capture_file + ".info")
-            capture_rate = self.info["capture-rate"]
+            capture_rate = self.options.sample_rate
             self.__set_rx_from_file(capture_file, capture_rate)
             self._set_titlebar(capture_file)
             self._set_state("RUNNING")
@@ -1139,28 +1188,21 @@ class p25_rx_block (stdgui2.std_top_block):
 # assumes lock held or init
         idx = self.current_speed
         if self.fsk4_demod_connected:
-#           self.disconnect(self.mixer, self.lpf, self.arb_resampler, self.resamplers[idx], self.fm_demod, self.baseband_amp, self.symbol_filter, self.fsk4_demod, self.buffer, self.slicer, self.sink_s)
             if self.baseband_input:
                 self.disconnect(self.source, self.baseband_amp, self.float_resamplers[self.current_speed], self.symbol_filter)
             else:
-                self.disconnect(self.mixer, self.lpf, self.arb_resampler, self.resamplers[idx], self.fm_demod, self.baseband_amp, self.symbol_filter)
-            self.disconnect(self.symbol_filter, self.fsk4_demod, self.buffer, self.slicer, self.sink_s, self.sink_imbe)
-            if self.options.raw_symbols:
-                self.disconnect(self.slicer, self.sink_sf)
+                self.disconnect(self.arb_resampler, self.resamplers[idx], self.fm_demod, self.baseband_amp, self.symbol_filter)
+            self.disconnect(self.symbol_filter, self.fsk4_demod, self.buffer)
             self.fsk4_demod_connected = False
         if self.psk_demod_connected:
-            self.disconnect(self.mixer, self.lpf, self.arb_resampler, self.resamplers[idx], self.agc, self.symbol_filter_c, self.clock, self.diffdec, self.to_float, self.rescale, self.buffer, self.slicer, self.sink_s, self.sink_imbe)
-            if self.options.raw_symbols:
-                self.disconnect(self.slicer, self.sink_sf)
+            self.disconnect(self.arb_resampler, self.resamplers[idx], self.agc, self.symbol_filter_c, self.clock, self.diffdec, self.to_float, self.rescale, self.buffer)
             self.psk_demod_connected = False
 
     def connect_psk_demod(self):
 # assumes lock held or init
         self.disconnect_demods()
         idx = self.current_speed
-        self.connect(self.mixer, self.lpf, self.arb_resampler, self.resamplers[idx], self.agc, self.symbol_filter_c, self.clock, self.diffdec, self.to_float, self.rescale, self.buffer, self.slicer, self.sink_s, self.sink_imbe)
-        if self.options.raw_symbols:
-            self.connect(self.slicer, self.sink_sf)
+        self.connect(self.arb_resampler, self.resamplers[idx], self.agc, self.symbol_filter_c, self.clock, self.diffdec, self.to_float, self.rescale, self.buffer)
         self.psk_demod_connected = True
 
     def connect_fsk4_demod(self):
@@ -1170,10 +1212,8 @@ class p25_rx_block (stdgui2.std_top_block):
         if self.baseband_input:
             self.connect(self.source, self.baseband_amp, self.float_resamplers[self.current_speed], self.symbol_filter)
         else:
-            self.connect(self.mixer, self.lpf, self.arb_resampler, self.resamplers[idx], self.fm_demod, self.baseband_amp, self.symbol_filter)
-        self.connect(self.symbol_filter, self.fsk4_demod, self.buffer, self.slicer, self.sink_s, self.sink_imbe)
-        if self.options.raw_symbols:
-            self.connect(self.slicer, self.sink_sf)
+            self.connect(self.arb_resampler, self.resamplers[idx], self.fm_demod, self.baseband_amp, self.symbol_filter)
+        self.connect(self.symbol_filter, self.fsk4_demod, self.buffer)
         self.fsk4_demod_connected = True
 
     def connect_demods(self):
diff --git a/op25/gr-op25_repeater/apps/tdma/duid.py b/op25/gr-op25_repeater/apps/tdma/duid.py
index 0468e1c..ae40454 100644
--- a/op25/gr-op25_repeater/apps/tdma/duid.py
+++ b/op25/gr-op25_repeater/apps/tdma/duid.py
@@ -21,21 +21,6 @@
 import numpy as np
 from bit_utils import *
 
-duid_str = {}
-duid_str[0] = "4v"
-duid_str[3] = "sacch w"
-duid_str[6] = "2v"
-duid_str[9] = "facch w"
-duid_str[12] = "sacch w/o"
-duid_str[15] = "facch w/o"
-
-duid_map = {}
-def mk_duid_lookup():
-	g = np.array(np.mat('1 0 0 0 1 1 0 1; 0 1 0 0 1 0 1 1; 0 0 1 0 1 1 1 0; 0 0 0 1 0 1 1 1'))
-	for i in xrange(16):
-		codeword = mk_str(np.dot(mk_array(i, 4), g))
-		duid_map[codeword] = i
-
 def extract_duid(b):
 	duid0 = b[10]	# duid 3,2
 	duid1 = b[47]	# duid 1,0
@@ -45,9 +30,29 @@ def extract_duid(b):
 	va = mk_array(v, 8)
 	return mk_str(va)
 
-def decode_duid(burst):
-	try:
-		b = duid_str[duid_map[extract_duid(burst)]]
-	except:	# FIXME: find closest matching codeword
-		b = 'unknown' + extract_duid(burst)
-	return b
+def mk_duid_lookup():
+	duid_map = {}
+	g = np.array(np.mat('1 0 0 0 1 1 0 1; 0 1 0 0 1 0 1 1; 0 0 1 0 1 1 1 0; 0 0 0 1 0 1 1 1'))
+	for i in xrange(16):
+		codeword = mk_str(np.dot(mk_array(i, 4), g))
+		duid_map[codeword] = i
+	return duid_map
+
+class p25p2_duid(object):
+	def __init__(self):
+		self.duid_str = {}
+		self.duid_str[0] = "4v"
+		self.duid_str[3] = "sacch w"
+		self.duid_str[6] = "2v"
+		self.duid_str[9] = "facch w"
+		self.duid_str[12] = "sacch w/o"
+		self.duid_str[15] = "facch w/o"
+
+		self.duid_map = mk_duid_lookup()
+
+	def decode_duid(self, burst):
+		try:
+			b = self.duid_str[self.duid_map[extract_duid(burst)]]
+		except:	# FIXME: find closest matching codeword
+			b = 'unknown' + extract_duid(burst)
+		return b
diff --git a/op25/gr-op25_repeater/apps/tdma/isch.py b/op25/gr-op25_repeater/apps/tdma/isch.py
index 6a6a4c0..86b215a 100644
--- a/op25/gr-op25_repeater/apps/tdma/isch.py
+++ b/op25/gr-op25_repeater/apps/tdma/isch.py
@@ -21,15 +21,6 @@
 import numpy as np
 from bit_utils import *
 
-isch_map = {}
-
-def mk_isch_lookup():
-	g = np.array(np.mat('1 0 0 0 1 0 0 0 0 0 0 1 0 1 1 0 1 1 0 0 1 1 1 0 0 0 1 1 0 1 1 0 1 1 0 1 0 1 1 1; 0 0 1 0 0 0 0 0 0 0 0 1 1 1 0 1 1 1 1 1 1 1 0 1 0 1 0 0 1 1 1 1 0 1 1 0 0 1 0 0; 0 0 0 1 0 0 0 0 0 0 0 0 1 1 1 1 0 1 0 0 1 0 1 1 0 0 0 1 0 1 1 1 0 1 0 1 1 0 0 0; 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 0 1 1 0 1 1 1 1 0 1 1 0 1 0 0 0 1 1 0 0 0 1 1 1 0; 0 0 0 0 0 0 1 0 0 0 0 0 1 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 0 1 1 1 1 1 1 1 1 1 1 1; 0 0 0 0 1 0 0 1 0 0 0 0 0 1 0 0 1 0 0 0 1 1 0 1 1 0 0 1 1 0 1 1 0 1 1 1 0 0 1 0; 0 0 0 0 0 0 0 0 1 0 0 1 1 1 0 1 1 0 1 0 0 0 1 1 1 0 1 0 0 0 0 1 0 1 1 1 0 0 0 1; 0 0 0 0 0 0 0 0 0 1 0 1 1 0 0 0 1 1 0 0 1 0 1 1 1 0 1 0 1 0 1 0 0 1 0 0 1 1 1 0; 0 0 0 0 0 0 0 0 0 0 1 1 0 1 0 0 0 0 1 1 1 1 0 1 1 0 0 0 0 1 0 1 1 0 0 1 0 1 1 1'))
-	c0 = 0x184229d461L
-	for i in xrange(0, 2**7):
-		codeword = mk_int(np.dot(mk_array(i, 9), g)) ^ c0
-		isch_map['%x' % codeword] = i
-
 def mk_isch(v):
 	v1 = v & 3
 	v = v >> 2
@@ -42,15 +33,28 @@ def mk_isch(v):
 	v5 = v & 3
 	return v4, v3, v2, v1
 
-def decode_isch(syms):
-	sync0 = 0x575d57f7ff
-	v = mk_int(dibits_to_bits(syms))
-	vp = '%x' % v
-	isch = 'unknown'
-	if v == sync0:
-		return -2, -2, -2, -2
-	if vp in isch_map:
-		chn,loc,fr,cnt = mk_isch(isch_map[vp])
-		return chn, loc, fr, cnt
-	# FIXME: if bit error(s), locate closest matching codeword
-	return -1, -1, -1, -1
+class p25p2_isch(object):
+	def __init__(self):
+		self.isch_map = self.mk_isch_lookup()
+
+	def mk_isch_lookup(self):
+		isch_map = {}
+		g = np.array(np.mat('1 0 0 0 1 0 0 0 0 0 0 1 0 1 1 0 1 1 0 0 1 1 1 0 0 0 1 1 0 1 1 0 1 1 0 1 0 1 1 1; 0 0 1 0 0 0 0 0 0 0 0 1 1 1 0 1 1 1 1 1 1 1 0 1 0 1 0 0 1 1 1 1 0 1 1 0 0 1 0 0; 0 0 0 1 0 0 0 0 0 0 0 0 1 1 1 1 0 1 0 0 1 0 1 1 0 0 0 1 0 1 1 1 0 1 0 1 1 0 0 0; 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 0 1 1 0 1 1 1 1 0 1 1 0 1 0 0 0 1 1 0 0 0 1 1 1 0; 0 0 0 0 0 0 1 0 0 0 0 0 1 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 0 1 1 1 1 1 1 1 1 1 1 1; 0 0 0 0 1 0 0 1 0 0 0 0 0 1 0 0 1 0 0 0 1 1 0 1 1 0 0 1 1 0 1 1 0 1 1 1 0 0 1 0; 0 0 0 0 0 0 0 0 1 0 0 1 1 1 0 1 1 0 1 0 0 0 1 1 1 0 1 0 0 0 0 1 0 1 1 1 0 0 0 1; 0 0 0 0 0 0 0 0 0 1 0 1 1 0 0 0 1 1 0 0 1 0 1 1 1 0 1 0 1 0 1 0 0 1 0 0 1 1 1 0; 0 0 0 0 0 0 0 0 0 0 1 1 0 1 0 0 0 0 1 1 1 1 0 1 1 0 0 0 0 1 0 1 1 0 0 1 0 1 1 1'))
+		c0 = 0x184229d461L
+		for i in xrange(0, 2**7):
+			codeword = mk_int(np.dot(mk_array(i, 9), g)) ^ c0
+			isch_map['%x' % codeword] = i
+		return isch_map
+
+	def decode_isch(self, syms):
+		sync0 = 0x575d57f7ff
+		v = mk_int(dibits_to_bits(syms))
+		vp = '%x' % v
+		isch = 'unknown'
+		if v == sync0:
+			return -2, -2, -2, -2
+		if vp in self.isch_map:
+			chn,loc,fr,cnt = mk_isch(self.isch_map[vp])
+			return chn, loc, fr, cnt
+		# FIXME: if bit error(s), locate closest matching codeword
+		return -1, -1, -1, -1
diff --git a/op25/gr-op25_repeater/apps/tdma/tdma-decode.py b/op25/gr-op25_repeater/apps/tdma/tdma-decode.py
index 44a451e..d9fa602 100755
--- a/op25/gr-op25_repeater/apps/tdma/tdma-decode.py
+++ b/op25/gr-op25_repeater/apps/tdma/tdma-decode.py
@@ -35,8 +35,8 @@ import numpy as np
 from optparse import OptionParser
 
 from bit_utils import *
-from isch import mk_isch_lookup, decode_isch
-from duid import mk_duid_lookup, decode_duid
+import isch
+import duid
 import lfsr
 from vf import process_v
 
@@ -55,10 +55,10 @@ def main():
             sys.exit(1)
 	file = options.input_file
 
-	mk_isch_lookup()
-	mk_duid_lookup()
+	my_isch = isch.p25p2_isch()
+	my_duid = duid.p25p2_duid()
+	my_lfsr = lfsr.p25p2_lfsr(options.nac, options.sysid, options.wacn)
 	#print 'nac: %d' % options.nac
-	sreg = lfsr.p25p2_lfsr(options.nac, options.sysid, options.wacn)
 
 	d = open(file).read()
 
@@ -71,7 +71,7 @@ def main():
 	assert sync_start > 0	# unable to locate any sync sequence
 	superframe = -1
 	for i in xrange(sync_start, sync_start + (180*32), 180):
-		chn, loc, fr, cnt = decode_isch ( symbols [ i : i + 20 ])
+		chn, loc, fr, cnt = my_isch.decode_isch ( symbols [ i : i + 20 ])
 		if chn == 0 and loc == 0:
 			superframe = i
 			break
@@ -80,11 +80,11 @@ def main():
 	errors = 0
 	for i in xrange(superframe,len(symbols)-SUPERFRAME_LEN,SUPERFRAME_LEN):
 		syms1 = symbols[i + 10: i + SUPERFRAME_LEN + 10]
-		syms2 = np.array(syms1) ^ sreg.xorsyms
+		syms2 = np.array(syms1) ^ my_lfsr.xorsyms
 		for j in xrange(12):
 			if options.verbose:
 				print '%s superframe %d timeslot %d %s' % ('=' * 20, i, j, '=' * 20)
-			chn, loc, fr, cnt = decode_isch ( symbols [ i + (j*180) : i + (j*180) + 20 ])
+			chn, loc, fr, cnt = my_isch.decode_isch ( symbols [ i + (j*180) : i + (j*180) + 20 ])
 			if chn == -1:
 				if options.verbose:
 					print 'unknown isch codeword at %d' % (i + (j*180))
@@ -100,11 +100,16 @@ def main():
 
 			burst = syms1 [ (j*180) : (j*180) + 180 ]
 			burst_d= syms2 [ (j*180) : (j*180) + 180 ]
-			b = decode_duid(burst)
+			btype = my_duid.decode_duid(burst)
 			if options.verbose:
-				print 'burst at %d type %s' % (i + (j*180), b)
-			if b == '2v' or b == '4v':
-				process_v(burst_d, b)
+				print 'burst at %d type %s' % (i + (j*180), btype)
+			if btype == '2v' or btype == '4v':
+				process_v(burst_d, btype)
+			elif not btype.startswith('unknown'):
+				maybe_sync = burst[79:79+21]
+				if btype.endswith(' w'):	# scrambled
+					burst = burst_d
+				# process_oemi(burst, btype)
 		if errors > 6:
 			if options.verbose:
 				print "too many successive errors, exiting at i=%d" % (i)
diff --git a/op25/gr-op25_repeater/apps/tdma/tdma_audio.py b/op25/gr-op25_repeater/apps/tdma/tdma_audio.py
index 3968bb9..7f709ed 100755
--- a/op25/gr-op25_repeater/apps/tdma/tdma_audio.py
+++ b/op25/gr-op25_repeater/apps/tdma/tdma_audio.py
@@ -20,7 +20,9 @@ class app_top_block(gr.top_block):
         IN = blocks.file_source(gr.sizeof_char, options.input_file)
 
         slotid = options.tdma_slotid
-        FRAMER = op25_repeater.p25p2_frame(0, slotid)
+        msgq = gr.msg_queue(2)
+        do_msgq  = False
+        FRAMER = op25_repeater.p25p2_frame(0, slotid, do_msgq, msgq)
         FRAMER.set_xormask(xor_mask)
 
         S2F = blocks.short_to_float()
diff --git a/op25/gr-op25_repeater/apps/trunking.py b/op25/gr-op25_repeater/apps/trunking.py
index 08d7993..7fa9ddf 100644
--- a/op25/gr-op25_repeater/apps/trunking.py
+++ b/op25/gr-op25_repeater/apps/trunking.py
@@ -396,6 +396,8 @@ class rx_ctl (object):
         self.TSYS_HOLD_TIME = 3.0	# TODO: make more configurable
         self.wait_until = time.time()
         self.configs = {}
+        self.last_tdma_vf = 0
+        self.P2_GRACE_TIME = 1.0	# TODO: make more configurable
 
         if conf_file:
             if conf_file.endswith('.tsv'):
@@ -524,7 +526,7 @@ class rx_ctl (object):
         nac = (ord(s[0]) << 8) + ord(s[1])
         if nac == 0xffff:
             # TDMA
-            self.update_state('duid%d' % type, curr_time)
+            self.update_state('tdma_duid%d' % type, curr_time)
             return
         s = s[2:]
         if self.debug > 10:
@@ -575,7 +577,7 @@ class rx_ctl (object):
         new_slot = None
 
         if command == 'timeout' or command == 'duid15':
-            if self.current_state != self.states.CC:
+            if self.current_state != self.states.CC and curr_time - self.last_tdma_vf > self.P2_GRACE_TIME:
                 new_state = self.states.CC
                 new_frequency = tsys.trunk_cc
         elif command == 'update':
@@ -588,16 +590,18 @@ class rx_ctl (object):
                     new_state = self.states.TO_VC
                     self.current_tgid = new_tgid
                     new_slot = tdma_slot
-        elif command == 'duid3':
+        elif command == 'duid3' or command == 'tdma_duid3':
             if self.current_state != self.states.CC:
                 new_state = self.states.CC
                 new_frequency = tsys.trunk_cc
-        elif command == 'duid0' or command == 'duid5' or command == 'duid10':
+        elif command == 'duid0' or command == 'duid5' or command == 'duid10' or command == 'tdma_duid5':
             if self.state == self.states.TO_VC:
                 new_state = self.states.VC
             self.tgid_hold = self.current_tgid
             self.tgid_hold_until = max(curr_time + self.TGID_HOLD_TIME, self.tgid_hold_until)
             self.wait_until = curr_time + self.TSYS_HOLD_TIME
+            if command == 'tdma_duid5':
+                self.last_tdma_vf = curr_time
         elif command == 'duid7' or command == 'duid12':
             pass
         elif command == 'set_hold':
diff --git a/op25/gr-op25_repeater/include/op25_repeater/gardner_costas_cc.h b/op25/gr-op25_repeater/include/op25_repeater/gardner_costas_cc.h
index 0bfea31..261ca11 100644
--- a/op25/gr-op25_repeater/include/op25_repeater/gardner_costas_cc.h
+++ b/op25/gr-op25_repeater/include/op25_repeater/gardner_costas_cc.h
@@ -47,6 +47,7 @@ namespace gr {
        * creating new instances.
        */
       static sptr make(float samples_per_symbol, float gain_mu, float gain_omega, float alpha, float beta, float max_freq, float min_freq);
+      virtual void set_omega(float omega) {}
     };
 
   } // namespace op25_repeater
diff --git a/op25/gr-op25_repeater/lib/gardner_costas_cc_impl.cc b/op25/gr-op25_repeater/lib/gardner_costas_cc_impl.cc
index e73cd26..43e2ecc 100644
--- a/op25/gr-op25_repeater/lib/gardner_costas_cc_impl.cc
+++ b/op25/gr-op25_repeater/lib/gardner_costas_cc_impl.cc
@@ -40,6 +40,7 @@ static const float M_TWOPI = 2 * M_PI;
 #define VERBOSE_GARDNER 0    // Used for debugging symbol timing loop
 #define VERBOSE_COSTAS 0     // Used for debugging phase and frequency tracking
 static const gr_complex PT_45 = gr_expj( M_PI / 4.0 );
+static const int NUM_COMPLEX=100;
 
 namespace gr {
   namespace op25_repeater {
@@ -60,11 +61,12 @@ namespace gr {
               gr::io_signature::make(1, 1, sizeof(gr_complex))),
     d_mu(0),
     d_gain_omega(gain_omega),
+    d_omega_rel(0.005),
     d_gain_mu(gain_mu),
     d_last_sample(0), d_interp(new gr::filter::mmse_fir_interpolator_cc()),
     //d_verbose(gr::prefs::singleton()->get_bool("gardner_costas_cc", "verbose", false)),
     d_verbose(false),
-    d_dl(0),
+    d_dl(new gr_complex[NUM_COMPLEX]),
     d_dl_index(0),
     d_alpha(alpha), d_beta(beta), 
     d_interp_counter(0),
@@ -80,18 +82,11 @@ namespace gr {
      */
     gardner_costas_cc_impl::~gardner_costas_cc_impl()
     {
+  delete [] d_dl;
   delete d_interp;
-    if (d_dl) {
-	delete d_dl;
-	d_dl = 0;
-    }
     }
 
 void gardner_costas_cc_impl::set_omega (float omega) {
-    if (d_dl) {
-	delete d_dl;
-	d_dl = 0;
-    }
     assert (omega >= 2.0);
     d_omega = omega;
     d_min_omega = omega*(1.0 - d_omega_rel);
@@ -99,8 +94,9 @@ void gardner_costas_cc_impl::set_omega (float omega) {
     d_omega_mid = 0.5*(d_min_omega+d_max_omega);
     d_twice_sps = 2 * (int) ceilf(d_omega);
     int num_complex = std::max(d_twice_sps*2, 16);
-    d_dl = new gr_complex[num_complex];
-    memset(d_dl, 0, num_complex * sizeof(gr_complex));
+    if (num_complex > NUM_COMPLEX)
+        fprintf(stderr, "gardner_costas_cc: warning omega %f size %d exceeds NUM_COMPLEX %d\n", omega, num_complex, NUM_COMPLEX);
+    memset(d_dl, 0, NUM_COMPLEX * sizeof(gr_complex));
 }
 
 
diff --git a/op25/gr-op25_repeater/lib/p25p2_frame_impl.cc b/op25/gr-op25_repeater/lib/p25p2_frame_impl.cc
index cc0b20d..b9576b4 100644
--- a/op25/gr-op25_repeater/lib/p25p2_frame_impl.cc
+++ b/op25/gr-op25_repeater/lib/p25p2_frame_impl.cc
@@ -47,7 +47,8 @@ namespace gr {
               gr::io_signature::make(1, 1, sizeof(short))),
 	p2tdma(slotid, debug, &output_queue_decode),
 	d_do_msgq(do_msgq),
-	d_msg_queue(msgq)
+	d_msg_queue(msgq),
+	d_debug(debug)
     {}
 
     /*
diff --git a/op25/gr-op25_repeater/lib/p25p2_frame_impl.h b/op25/gr-op25_repeater/lib/p25p2_frame_impl.h
index 927b8d9..ba025ca 100644
--- a/op25/gr-op25_repeater/lib/p25p2_frame_impl.h
+++ b/op25/gr-op25_repeater/lib/p25p2_frame_impl.h
@@ -41,6 +41,7 @@ namespace gr {
 	bool d_do_msgq;
 	gr::msg_queue::sptr d_msg_queue;
       void queue_msg(int duid);
+      int d_debug;
 
      public:
 	typedef std::vector<bool> bit_vector;