From 13c14c799f6d857678dc1df640b404715d500ff0 Mon Sep 17 00:00:00 2001
From: Nikos Karamolegkos <nkaramolegos@csd.uoc.gr>
Date: Wed, 14 Mar 2018 00:43:48 +0200
Subject: [PATCH] Add UDP socket for the message source block

---
 grc/lora_message_socket_source.xml   | 14 ++++-
 include/lora/message_socket_source.h |  2 +-
 lib/message_socket_source_impl.cc    | 81 ++++++++++++++++++++++++++--
 lib/message_socket_source_impl.h     | 12 ++++-
 4 files changed, 102 insertions(+), 7 deletions(-)

diff --git a/grc/lora_message_socket_source.xml b/grc/lora_message_socket_source.xml
index a8e7847..cdc6554 100644
--- a/grc/lora_message_socket_source.xml
+++ b/grc/lora_message_socket_source.xml
@@ -4,7 +4,7 @@
 	<key>lora_message_socket_source</key>
 	<category>[LoRa]</category>
 	<import>import lora</import>
-	<make>lora.message_socket_source($addr, $port, $mtu)</make>
+	<make>lora.message_socket_source($addr, $port, $mtu, $payload_len)</make>
 
 	<param>
 		<name>UDP IP Address</name>
@@ -27,4 +27,16 @@
 		<type>int</type>
 	</param>
 
+	<param>
+		<name>Payload length</name>
+		<key>payload_len</key>
+		<value>100</value>
+		<type>int</type>
+	</param>
+
+	<source>
+		<name>out</name>
+		<type>message</type>
+	</source>
+
 </block>
diff --git a/include/lora/message_socket_source.h b/include/lora/message_socket_source.h
index f4c821f..187b883 100644
--- a/include/lora/message_socket_source.h
+++ b/include/lora/message_socket_source.h
@@ -703,7 +703,7 @@ namespace gr {
        * class. lora::message_socket_source::make is the public interface for
        * creating new instances.
        */
-      static sptr make(const std::string& addr, uint16_t port, size_t mtu);
+      static sptr make(const std::string& addr, uint16_t port, size_t mtu, size_t payload_len);
     };
 
   } // namespace lora
diff --git a/lib/message_socket_source_impl.cc b/lib/message_socket_source_impl.cc
index bc2ffaa..78c94f4 100644
--- a/lib/message_socket_source_impl.cc
+++ b/lib/message_socket_source_impl.cc
@@ -22,6 +22,8 @@
 #include "config.h"
 #endif
 
+#include <lora/loratap.h>
+#include <lora/loraphy.h>
 #include <gnuradio/io_signature.h>
 #include "message_socket_source_impl.h"
 
@@ -32,24 +34,93 @@ namespace gr
 
     message_socket_source::sptr
     message_socket_source::make (const std::string& addr, uint16_t port,
-				 size_t mtu)
+				 size_t mtu, size_t payload_len)
     {
       return gnuradio::get_initial_sptr (
-	  new message_socket_source_impl (addr, port, mtu));
+	  new message_socket_source_impl (addr, port, mtu, payload_len));
     }
 
     /*
      * The private constructor
      */
     message_socket_source_impl::message_socket_source_impl (
-	const std::string& addr, uint16_t port, size_t mtu) :
+	const std::string& addr, uint16_t port, size_t mtu, size_t payload_len) :
 	    gr::block ("message_socket_source",
 		       gr::io_signature::make (0, 0, 0),
 		       gr::io_signature::make (0, 0, 0)),
 	    d_addr (addr),
 	    d_udp_port (port),
-	    d_mtu (mtu)
+	    d_mtu (mtu),
+	    d_payload_len (payload_len),
+	    d_running (true),
+	    d_layer (0)
     {
+      message_port_register_out (pmt::mp ("out"));
+      boost::shared_ptr<boost::thread> (
+	  new boost::thread (
+	      boost::bind (&message_socket_source_impl::msg_receive_udp,
+			   this)));
+
+    }
+
+    void
+    message_socket_source_impl::msg_receive_udp ()
+    {
+      int sock;
+      struct sockaddr_in sin;
+      struct sockaddr client_addr;
+      socklen_t client_addr_len;
+      ssize_t ret;
+      uint8_t *buf;
+      uint32_t bytes_num;
+
+      if ((sock = socket (AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1) {
+	perror ("opening UDP socket");
+	exit (EXIT_FAILURE);
+      }
+
+      memset (&client_addr, 0, sizeof(struct sockaddr));
+      memset (&sin, 0, sizeof(struct sockaddr_in));
+      sin.sin_family = AF_INET;
+      sin.sin_port = htons (d_udp_port);
+
+      if (inet_aton (d_addr.c_str (), &(sin.sin_addr)) == 0) {
+	printf ("Wrong IP address\n");
+	close (sock);
+	exit (EXIT_FAILURE);
+      }
+
+      if (bind (sock, (struct sockaddr *) &sin, sizeof(struct sockaddr_in))
+	  == -1) {
+	perror ("UDP bind");
+	close (sock);
+	exit (EXIT_FAILURE);
+      }
+
+      /* All good until now. Allocate buffer memory and proceed */
+      buf = new uint8_t[d_mtu];
+
+      while (d_running) {
+	ret = recvfrom (sock, buf, d_mtu, 0, &client_addr, &client_addr_len);
+	if (ret > 0) {
+	  bytes_num = (uint32_t) ret;
+	  if (bytes_num == (sizeof(loratap_header_t)+sizeof(loraphy_header_t)+ d_payload_len)) {
+	    pmt::pmt_t lora_tap;
+	    lora_tap = pmt::make_blob (buf, bytes_num);
+	    message_port_pub (pmt::mp ("out"), lora_tap);
+	  }
+
+	}
+	else {
+	  perror ("UDP recvfrom");
+	  close (sock);
+	  delete[] buf;
+	  exit (EXIT_FAILURE);
+	}
+      }
+      close (sock);
+      delete[] buf;
+      exit (EXIT_SUCCESS);
     }
 
     /*
@@ -57,6 +128,8 @@ namespace gr
      */
     message_socket_source_impl::~message_socket_source_impl ()
     {
+      d_running = false;
+      d_thread->join ();
     }
 
   } /* namespace lora */
diff --git a/lib/message_socket_source_impl.h b/lib/message_socket_source_impl.h
index 0deed56..27413aa 100644
--- a/lib/message_socket_source_impl.h
+++ b/lib/message_socket_source_impl.h
@@ -22,6 +22,9 @@
 #define INCLUDED_LORA_MESSAGE_SOCKET_SOURCE_IMPL_H
 
 #include <lora/message_socket_source.h>
+#include <sys/socket.h>
+#include <arpa/inet.h>
+#include <boost/thread.hpp>
 
 namespace gr
 {
@@ -34,10 +37,17 @@ namespace gr
       const std::string d_addr;
       const uint16_t d_udp_port;
       const size_t d_mtu;
+      const size_t d_payload_len;
+      bool d_running;
+      int d_layer;
+      boost::shared_ptr<boost::thread> d_thread;
+
+
+      void msg_receive_udp();
 
     public:
       message_socket_source_impl (const std::string& addr, uint16_t port,
-				  size_t mtu);
+				  size_t mtu, size_t payload_len);
       ~message_socket_source_impl ();
     };