163 lines
4.7 KiB
C++
163 lines
4.7 KiB
C++
/* -*- c++ -*- */
|
|
/*
|
|
* gr-satnogs: SatNOGS GNU Radio Out-Of-Tree Module
|
|
*
|
|
* Copyright (C) 2016, Libre Space Foundation <http://librespacefoundation.org/>
|
|
*
|
|
* This program 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 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#ifdef HAVE_CONFIG_H
|
|
#include "config.h"
|
|
#endif
|
|
|
|
#include <gnuradio/io_signature.h>
|
|
#include "udp_msg_source_impl.h"
|
|
#include <satnogs/log.h>
|
|
#include <ifaddrs.h>
|
|
#include <errno.h>
|
|
#include <sys/time.h>
|
|
#include <sys/types.h>
|
|
#include <sys/socket.h>
|
|
#include <netinet/in.h>
|
|
#include <arpa/inet.h>
|
|
|
|
namespace gr
|
|
{
|
|
namespace satnogs
|
|
{
|
|
|
|
udp_msg_source::sptr
|
|
udp_msg_source::make (const std::string& addr, uint16_t port, size_t mtu,
|
|
size_t type)
|
|
{
|
|
return gnuradio::get_initial_sptr (
|
|
new udp_msg_source_impl (addr, port, mtu, type));
|
|
}
|
|
|
|
/*
|
|
* The private constructor
|
|
*/
|
|
udp_msg_source_impl::udp_msg_source_impl (const std::string& addr,
|
|
uint16_t port, size_t mtu,
|
|
size_t type) :
|
|
gr::block ("udp_msg_source", gr::io_signature::make (0, 0, 0),
|
|
gr::io_signature::make (0, 0, 0)),
|
|
d_iface_addr (addr),
|
|
d_udp_port (port),
|
|
d_mtu (mtu),
|
|
d_type (type),
|
|
d_running (true)
|
|
{
|
|
message_port_register_out (pmt::mp ("msg"));
|
|
boost::shared_ptr<boost::thread> (
|
|
new boost::thread (
|
|
boost::bind (&udp_msg_source_impl::udp_msg_accepter, this)));
|
|
}
|
|
|
|
void
|
|
udp_msg_source_impl::udp_msg_accepter ()
|
|
{
|
|
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;
|
|
uint32_t uint_val;
|
|
uint32_t int_val;
|
|
|
|
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_iface_addr.c_str (), &(sin.sin_addr)) == 0) {
|
|
LOG_ERROR("Wrong IP address");
|
|
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;
|
|
switch(d_type){
|
|
case 0:
|
|
message_port_pub (pmt::mp ("msg"), pmt::make_blob (buf, bytes_num));
|
|
break;
|
|
case 1:
|
|
if(bytes_num < sizeof(uint32_t)){
|
|
continue;
|
|
}
|
|
memcpy(&uint_val, buf, sizeof(uint32_t));
|
|
message_port_pub (pmt::mp ("msg"),
|
|
pmt::from_uint64 (ntohl (uint_val)));
|
|
break;
|
|
case 2:
|
|
if (bytes_num < sizeof(int32_t)) {
|
|
continue;
|
|
}
|
|
memcpy(&int_val, buf, sizeof(int32_t));
|
|
message_port_pub (pmt::mp ("msg"),
|
|
pmt::from_long (ntohl (int_val)));
|
|
break;
|
|
default:
|
|
LOG_ERROR("Unsupported message type");
|
|
close (sock);
|
|
delete[] buf;
|
|
exit (EXIT_FAILURE);
|
|
}
|
|
}
|
|
else {
|
|
perror ("UDP recvfrom");
|
|
close (sock);
|
|
delete[] buf;
|
|
exit (EXIT_FAILURE);
|
|
}
|
|
}
|
|
close (sock);
|
|
delete[] buf;
|
|
exit (EXIT_SUCCESS);
|
|
}
|
|
|
|
/*
|
|
* Our virtual destructor.
|
|
*/
|
|
udp_msg_source_impl::~udp_msg_source_impl ()
|
|
{
|
|
d_running = false;
|
|
d_thread->join ();
|
|
}
|
|
|
|
} /* namespace satnogs */
|
|
} /* namespace gr */
|
|
|