gr-satnogs/lib/ax25_encoder_mb_impl.cc

/* -*- c++ -*- */
/*
 * gr-satnogs: SatNOGS GNU Radio Out-Of-Tree Module
 *
 *  Copyright (C) 2016,2018
 *  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 "ax25_encoder_mb_impl.h"
#include <satnogs/log.h>
#include <satnogs/ax25.h>

namespace gr {
namespace satnogs {

ax25_encoder_mb::sptr
ax25_encoder_mb::make(const std::string &dest_addr, uint8_t dest_ssid,
                      const std::string &src_addr, uint8_t src_ssid,
                      size_t preamble_len, size_t postamble_len,
                      bool scramble)
{
  return gnuradio::get_initial_sptr(
           new ax25_encoder_mb_impl(dest_addr, dest_ssid, src_addr, src_ssid,
                                    preamble_len, postamble_len, scramble));
}

/*
 * The private constructor
 */
ax25_encoder_mb_impl::ax25_encoder_mb_impl(const std::string &dest_addr,
    uint8_t dest_ssid,
    const std::string &src_addr,
    uint8_t src_ssid,
    size_t preamble_len,
    size_t postabmle_len,
    bool scramble) :
  gr::sync_block("ax25_encoder_mb", gr::io_signature::make(0, 0, 0),
                 gr::io_signature::make(1, 1, sizeof(uint8_t))),
  d_preamble_len(preamble_len),
  d_postamble_len(postabmle_len),
  d_scramble(scramble),
  d_remaining(0),
  d_produced(0),
  d_prev_bit(0x0),
  d_encoded_frame(
    new uint8_t[(preamble_len + postabmle_len
                 + (AX25_MAX_FRAME_LEN * 2)) * 8]),
  d_tmp_buf(
    new uint8_t[preamble_len + postabmle_len
                             + (AX25_MAX_FRAME_LEN * 2)]),
  d_addr_field(new uint8_t[AX25_MAX_ADDR_LEN]),
  d_lfsr(0x21, 0x0, 16)
{
  /* Input is a blob message containing the info field data */
  message_port_register_in(pmt::mp("info"));
  d_addr_len = ax25_create_addr_field(d_addr_field, dest_addr, dest_ssid,
                                      src_addr, src_ssid);
}

/*
 * Our virtual destructor.
 */
ax25_encoder_mb_impl::~ax25_encoder_mb_impl()
{
  delete [] d_encoded_frame;
  delete [] d_tmp_buf;
  delete [] d_addr_field;
}

int
ax25_encoder_mb_impl::work(int noutput_items,
                           gr_vector_const_void_star &input_items,
                           gr_vector_void_star &output_items)
{
  const uint8_t *info;
  size_t info_len;
  size_t max_avail;
  size_t len;
  size_t i;
  pmt::pmt_t msg;
  ax25_encode_status_t status;

  uint8_t *out = (uint8_t *) output_items[0];

  /* If all the frame samples have already be sent, wait for a new frame */
  if (d_remaining == 0) {
    d_produced = 0;
    d_prev_bit = 0x0;
    d_lfsr.reset();

    /* Block waiting from a new message from users */
    msg = delete_head_nowait(pmt::mp("info"));
    if (msg.get() == nullptr) {
      return 0;
    }
    info = (const uint8_t *) pmt::blob_data(msg);
    info_len = pmt::blob_length(msg);

    /* Prepare and encode the AX.25 frame */
    len = ax25_prepare_frame(d_tmp_buf, info, info_len, AX25_I_FRAME,
                             d_addr_field, d_addr_len, 0, 1,
                             d_preamble_len, d_postamble_len);

    /* Perform bit stuffing */
    status = ax25_bit_stuffing(d_encoded_frame, &d_remaining, d_tmp_buf,
                               len, d_preamble_len, d_postamble_len);
    if (status != AX25_ENC_OK) {
      LOG_ERROR("AX.25 encoding failed");
      d_remaining = 0;
      return 0;
    }

    /*Perform scrambling if the user asked for it */
    if (d_scramble) {
      for (i = 0; i < d_remaining; i++) {
        d_encoded_frame[i] = d_lfsr.next_bit_scramble(d_encoded_frame[i]);
      }
      /* Allow the LFSR to pop all its bits */
      d_remaining += 16;
      for (; i < d_remaining; i++) {
        d_encoded_frame[i] = d_lfsr.next_bit_scramble(0x0);
      }
    }
    /* Append a zero byte at the end */
    memset(&d_encoded_frame[d_remaining], 0, 8);
    d_remaining += 8;
  }

  /* If this is the first part of the frame add the start of burst tag*/
  if (d_produced == 0) {
    add_sob(nitems_written(0));
  }
  max_avail = std::min(d_remaining, (size_t) noutput_items);

  /* Perform NRZI encoding */
  for (i = 0; i < max_avail; i++) {
    out[i] = ((0x1 & ~d_encoded_frame[i + d_produced]) + d_prev_bit) % 2;
    d_prev_bit = out[i];
  }

  d_remaining -= max_avail;
  d_produced += max_avail;
  if (d_remaining == 0) {
    add_eob(nitems_written(0) + max_avail);
  }
  return (int) max_avail;
}


void
ax25_encoder_mb_impl::add_sob(uint64_t item)
{
  static const pmt::pmt_t sob_key = pmt::string_to_symbol("tx_sob");
  static const pmt::pmt_t value = pmt::PMT_T;
  static const pmt::pmt_t srcid = pmt::string_to_symbol(alias());
  add_item_tag(0, item, sob_key, value, srcid);
}

void
ax25_encoder_mb_impl::add_eob(uint64_t item)
{
  static const pmt::pmt_t eob_key = pmt::string_to_symbol("tx_eob");
  static const pmt::pmt_t value = pmt::PMT_T;
  static const pmt::pmt_t srcid = pmt::string_to_symbol(alias());
  add_item_tag(0, item, eob_key, value, srcid);
}

} /* namespace satnogs */
} /* namespace gr */