patch for p25p2 audio thx Graham (boatbod)

3 files changed, 56 insertions, 3 deletions

diff --git a/op25/gr-op25_repeater/lib/p25_frame_assembler_impl.cc b/op25/gr-op25_repeater/lib/p25_frame_assembler_impl.cc
index 18a3dff..27d0d68 100644
--- a/op25/gr-op25_repeater/lib/p25_frame_assembler_impl.cc
+++ b/op25/gr-op25_repeater/lib/p25_frame_assembler_impl.cc
@@ -90,7 +90,7 @@ static const int MAX_IN = 1;	// maximum number of input streams
 	p1fdma(udp_host, port, debug, do_imbe, do_output, do_msgq, queue, output_queue, do_audio_output),
 	d_do_audio_output(do_audio_output),
 	d_do_phase2_tdma(do_phase2_tdma),
-	p2tdma(0, debug, output_queue),
+	p2tdma(udp_host, port, 0, debug, output_queue),
 	d_do_msgq(do_msgq),
 	d_msg_queue(queue)
 {
diff --git a/op25/gr-op25_repeater/lib/p25p2_tdma.cc b/op25/gr-op25_repeater/lib/p25p2_tdma.cc
index 8a64216..c3c2970 100644
--- a/op25/gr-op25_repeater/lib/p25p2_tdma.cc
+++ b/op25/gr-op25_repeater/lib/p25p2_tdma.cc
@@ -23,6 +23,7 @@
 #include <string>
 #include <iostream>
 #include <assert.h>
+#include <errno.h>
 
 #include "p25p2_duid.h"
 #include "p25p2_sync.h"
@@ -65,7 +66,11 @@ static bool crc12_ok(const uint8_t bits[], unsigned int len) {
 	return (crc == crc12(bits,len));
 }
 
-p25p2_tdma::p25p2_tdma(int slotid, int debug, std::deque<int16_t> &qptr) :	// constructor
+p25p2_tdma::p25p2_tdma(const char* udp_host, int port, int slotid, int debug, std::deque<int16_t> &qptr) :	// constructor
+	write_bufp(0),
+	write_sock(0),
+	d_udp_host(udp_host),
+	d_port(port),
 	tdma_xormask(new uint8_t[SUPERFRAME_SIZE]),
 	symbols_received(0),
 	packets(0),
@@ -75,6 +80,9 @@ p25p2_tdma::p25p2_tdma(int slotid, int debug, std::deque<int16_t> &qptr) :	// co
 	crc_errors(0),
 	p2framer()
 {
+	if (port > 0)
+		init_sock(d_udp_host, d_port);
+
 	assert (slotid == 0 || slotid == 1);
 	mbe_initMbeParms (&cur_mp, &prev_mp, &enh_mp);
 }
@@ -93,6 +101,9 @@ void p25p2_tdma::set_slotid(int slotid)
 
 p25p2_tdma::~p25p2_tdma()	// destructor
 {
+	if (write_sock > 0)
+		close(write_sock);
+
 	delete[](tdma_xormask);
 }
 
@@ -184,6 +195,7 @@ void p25p2_tdma::handle_voice_frame(const uint8_t dibits[])
 	if (rc == 0)
 		software_decoder.decode_tap(cur_mp.L, K, cur_mp.w0, &cur_mp.Vl[1], &cur_mp.Ml[1]);
 	audio_samples *samples = software_decoder.audio();
+	write_bufp = 0;
 	for (int i=0; i < NSAMP_OUTPUT; i++) {
 		if (samples->size() > 0) {
 			snd = (int16_t)(samples->front());
@@ -191,8 +203,19 @@ void p25p2_tdma::handle_voice_frame(const uint8_t dibits[])
 		} else {
 			snd = 0;
 		}
+		write_buf[write_bufp++] = snd & 0xFF ;
+		write_buf[write_bufp++] = snd >> 8;
+#if 0
 		output_queue_decode.push_back(snd);
+#endif
 	}
+	if (write_sock > 0) {
+		if (write_bufp >= 0) { 
+			sendto(write_sock, write_buf, write_bufp, 0, (struct sockaddr *)&write_sock_addr, sizeof(write_sock_addr));
+			write_bufp = 0;
+		}
+	}
+
 	mbe_moveMbeParms (&cur_mp, &prev_mp);
 	mbe_moveMbeParms (&cur_mp, &enh_mp);
 }
@@ -249,3 +272,23 @@ int p25p2_tdma::handle_packet(const uint8_t dibits[])
 	}
 	return rc;
 }
+
+void p25p2_tdma::init_sock(const char* udp_host, int udp_port)
+{
+        memset (&write_sock_addr, 0, sizeof(write_sock_addr));
+        write_sock = socket(PF_INET, SOCK_DGRAM, 17);   // UDP socket
+        if (write_sock < 0) {
+                fprintf(stderr, "op25_ambe_vocoder: socket: %d\n", errno);
+                write_sock = 0;
+		return;
+        }
+        if (!inet_aton(udp_host, &write_sock_addr.sin_addr)) {
+                fprintf(stderr, "op25_ambe_vocoder: inet_aton: bad IP address\n");
+		close(write_sock);
+		write_sock = 0;
+		return;
+	}
+        write_sock_addr.sin_family = AF_INET;
+        write_sock_addr.sin_port = htons(udp_port);
+}
+
diff --git a/op25/gr-op25_repeater/lib/p25p2_tdma.h b/op25/gr-op25_repeater/lib/p25p2_tdma.h
index 0d386f6..d7e30c3 100644
--- a/op25/gr-op25_repeater/lib/p25p2_tdma.h
+++ b/op25/gr-op25_repeater/lib/p25p2_tdma.h
@@ -22,6 +22,9 @@
 
 #include <stdint.h>
 #include <deque>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
 #include "mbelib.h"
 #include "imbe_decoder.h"
 #include "software_imbe_decoder.h"
@@ -34,7 +37,7 @@ class p25p2_tdma;
 class p25p2_tdma
 {
 public:
-	p25p2_tdma(int slotid, int debug, std::deque<int16_t> &qptr) ;	// constructor
+	p25p2_tdma(const char* udp_host, int port, int slotid, int debug, std::deque<int16_t> &qptr) ;	// constructor
 	int handle_packet(const uint8_t dibits[]) ;
 	void set_slotid(int slotid);
 	uint8_t* tdma_xormask;
@@ -48,6 +51,12 @@ private:
 	p25p2_sync sync;
 	p25p2_duid duid;
 	p25p2_vf vf;
+	int write_bufp;
+	int write_sock;
+	struct sockaddr_in write_sock_addr;
+	char write_buf[512];
+	const char* d_udp_host;
+	int d_port;
 	int d_slotid;
 	mbe_parms cur_mp;
 	mbe_parms prev_mp;
@@ -63,5 +72,6 @@ private:
 	int handle_acch_frame(const uint8_t dibits[], bool fast) ;
 	void handle_voice_frame(const uint8_t dibits[]) ;
 	int process_mac_pdu(const uint8_t byte_buf[], unsigned int len) ;
+	void init_sock(const char* udp_host, int udp_port);
 };
 #endif /* INCLUDED_P25P2_TDMA_H */