gr-satnogs/lib/udp_msg_source_impl.cc

/* -*- 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 */