msg_arbiter.cpp

Go to the documentation of this file.

/*
 *  SatNOGS-COMMS MCU software
 *
 *  Copyright (C) 2023, Libre Space Foundation <http://libre.space>
 *
 *  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/>.
 *
 *  SPDX-License-Identifier: GNU General Public License v3.0 or later
 */
 
#include "msg_arbiter.hpp"
#include "callbacks.hpp"
#include "error_handler.hpp"
#include "io.hpp"
#include "io_wdg.hpp"
#include "mission.hpp"
#include "settings.hpp"
#include "telecommand.hpp"
#include "telemetry.hpp"
#include "tests/test.hpp"
#include "time.hpp"
#include <osdlp/uslp.hpp>
#include <psa/crypto.h>
#include <satnogs-comms-lib/version.hpp>
#include <string>
#include <zephyr/task_wdt/task_wdt.h>
 
namespace satnogs::comms
{
 
static constexpr std::array<uint8_t, 32>
sha256_key_hex2bytes()
{
  std::array<uint8_t, 32> key = {0};
  static_assert(std::char_traits<char>::length(CONFIG_TLC_SHA256_KEY) == 64,
                "Hash should be exactly 64 hex annotated chars");
  for (unsigned int i = 0; i < 64; i += 2) {
    char s[2]  = {CONFIG_TLC_SHA256_KEY[i], CONFIG_TLC_SHA256_KEY[i + 1]};
    char byte  = (char)strtol(s, NULL, 16);
    key[i / 2] = byte;
  }
  return key;
}
 
static struct k_thread msg_arbiter_thread_data;
K_THREAD_STACK_DEFINE(msg_arbiter_thread_stack, CONFIG_MSG_ARBITER_STACK_SIZE);
 
static char rx_msgq_buffer[CONFIG_RX_MSGQ_SIZE * sizeof(msg_arbiter::msg)];
static struct k_msgq rx_msgq;
 
static char
    can1_tx_msgq_buffer[CONFIG_CAN1_TX_MSGQ_SIZE * sizeof(msg_arbiter::msg)];
static struct k_msgq can1_tx_msgq;
 
static char
    can2_tx_msgq_buffer[CONFIG_CAN2_TX_MSGQ_SIZE * sizeof(msg_arbiter::msg)];
static struct k_msgq can2_tx_msgq;
 
static char
    uhf_tx_msgq_buffer[CONFIG_UHF_TX_MSGQ_SIZE * sizeof(msg_arbiter::msg)];
static struct k_msgq uhf_tx_msgq;
 
static char
    sband_tx_msgq_buffer[CONFIG_SBAND_TX_MSGQ_SIZE * sizeof(msg_arbiter::msg)];
static struct k_msgq sband_tx_msgq;
 
namespace scl = satnogs::comms::lib;
 
static const auto &tlc_key = sha256_key_hex2bytes();
 
static msg_arbiter::msg response;
static msg_arbiter::msg space_packet;
static msg_arbiter::msg fw_response;

int
msg_arbiter::push(const msg_arbiter::msg &m, k_timeout_t wait)
{
  return k_msgq_put(&rx_msgq, &m, wait);
}

int
msg_arbiter::pull(msg_arbiter::msg &m, subsys s, k_timeout_t wait)
{
  switch (s) {
  case subsys::CAN1:
    return k_msgq_get(&can1_tx_msgq, &m, wait);
  case subsys::CAN2:
    return k_msgq_get(&can2_tx_msgq, &m, wait);
  case subsys::RADIO_UHF:
    return k_msgq_get(&uhf_tx_msgq, &m, wait);
  case subsys::RADIO_SBAND:
    return k_msgq_get(&sband_tx_msgq, &m, wait);
  default:
    throw scl::inval_arg_exception(__FILE__, __LINE__);
  }
}

int
msg_arbiter::fwd(const msg_arbiter::msg &m, subsys s, k_timeout_t wait)
{
  switch (s) {
  case subsys::CAN1: {
#if CONFIG_IO_CAN1_ENABLE
    return k_msgq_put(&can1_tx_msgq, &m, wait);
#else
    throw scl::inval_arg_exception(__FILE__, __LINE__);
#endif
  }
  case subsys::CAN2: {
#if CONFIG_IO_CAN2_ENABLE
    return k_msgq_put(&can2_tx_msgq, &m, wait);
#else
    throw scl::inval_arg_exception(__FILE__, __LINE__);
#endif
  }
  case subsys::RADIO_UHF:
    return k_msgq_put(&uhf_tx_msgq, &m, wait);
  case subsys::RADIO_SBAND:
    return k_msgq_put(&sband_tx_msgq, &m, wait);
  default:
    throw scl::inval_arg_exception(__FILE__, __LINE__);
  }
}
 
int
msg_arbiter::fwd(uint8_t mapid, uint8_t vcid, const msg_arbiter::msg &m,
                 subsys s, k_timeout_t wait)
{
  osdlp::tf_primary_header uslp_tf_hdr(
      CONFIG_CCSDS_SPACECRAFT_ID,
      osdlp::tf_source_destination_id::SOURCE_TRANSFER_FRAME, vcid, mapid,
      osdlp::tf_bypass_sequence_control_flag::SEQUENCE_CONTROLLED_QOS, 0,
      osdlp::tf_operational_control_field_flag::OCF_NOT_PRESENT, 0);
 
  osdlp::tf_data_field_header uslp_tfdf_hdr;
  osdlp::uslp                 uslp_resp(uslp_tf_hdr, uslp_tfdf_hdr);
 
  auto uslp_len =
      uslp_resp.serialize(fw_response.data, CONFIG_MAX_MTU, m.data, m.len);
 
  fw_response.iface = m.iface;
  fw_response.len   = uslp_len;
  return fwd(fw_response, s, wait);
}

uint32_t
msg_arbiter::backpressure(subsys s)
{
  switch (s) {
  case subsys::CAN1:
    return k_msgq_num_used_get(&can1_tx_msgq);
  case subsys::CAN2:
    return k_msgq_num_used_get(&can2_tx_msgq);
  case subsys::RADIO_UHF:
    return k_msgq_num_used_get(&uhf_tx_msgq);
  case subsys::RADIO_SBAND:
    return k_msgq_num_used_get(&sband_tx_msgq);
  default:
    throw scl::inval_arg_exception(__FILE__, __LINE__);
  }
}

void
msg_arbiter::start()
{
  m_tid = k_thread_create(&msg_arbiter_thread_data, msg_arbiter_thread_stack,
                          K_THREAD_STACK_SIZEOF(msg_arbiter_thread_stack),
                          parse_thread, NULL, NULL, NULL,
                          CONFIG_MSG_ARBITER_PRIO, 0, K_NO_WAIT);
  if (!m_tid) {
    k_oops();
  }
  k_thread_name_set(m_tid, "msg_arbiter");
}
 
void
msg_arbiter::parse_thread(void *arg1, void *arg2, void *arg3)
{
  int          task_wdt_id = task_wdt_add(CONFIG_WATCHDOG_PERIOD_MSG_ARBITER,
                                          task_wdt_callback, (void *)k_current_get());
  msg_arbiter &arb         = msg_arbiter::get_instance();
  telecommand &tc          = telecommand::get_instance();
 
  while (1) {
    try {
      task_wdt_feed(task_wdt_id);
      if (k_msgq_get(&rx_msgq, &arb.m_msg, K_MSEC(1000)) == 0) {
        /*
         * SatNOGS-COMMS expects CCSDS USLP frames. However if none received, we
         * forward the message to the mission specific parser in case the
         * mission uses something custom
         */
 
        osdlp::tf_primary_header    uslp_tf_hdr;
        osdlp::tf_data_field_header uslp_tfdf_hdr;
 
        osdlp::uslp<> uslp(uslp_tf_hdr, uslp_tfdf_hdr);
        uslp.deserialize(arb.m_msg.data, arb.m_msg.len);
        if (uslp.get_is_valid_packet()) {
          /* Drop any frame that is not for this spacecraft */
          if (uslp_tf_hdr.get_spacecraft_id() != CONFIG_CCSDS_SPACECRAFT_ID) {
            continue;
          }
 
          /* COMMS is the final destination */
          if (uslp_tf_hdr.get_source_destination_id() !=
              osdlp::tf_source_destination_id::DESTINATION_TRANSFER_FRAME) {
            continue;
          }
 
          /*
           * Everything ok! Get the CCSDS space packet now, which is
           * encapsulated in the payload section of the USLP frame
           */
          const uint8_t *sp     = uslp.get_payload();
          size_t         sp_len = uslp.get_payload_length();
 
          if (tc.ccsds_frame_valid(sp, sp_len) == false) {
            continue;
          }
 
          // TC/TM cares only about the CCSDS Space packet portion
          space_packet.len   = std::min<size_t>(sp_len, CONFIG_MAX_MTU);
          space_packet.iface = arb.m_msg.iface;
          std::copy_n(sp, space_packet.len, space_packet.data);
 
          /*
           * SatNOGS-COMMS uses XTCE for the TC/TM try to decode the agreed TCs.
           * In case of failure, forward the space packet contents to the
           * mission specific parser as a last resort
           */
          if (tc.decode_ccsds_xtce(response, space_packet, task_wdt_id)) {
            // For now just response to the interface that we got the request
            response.iface = space_packet.iface;
 
            /* The response is a space packet. Make it a USLP */
            // FIXME Define properly the VCs
            osdlp::tf_primary_header uslp_resp_tf_hdr(
                CONFIG_CCSDS_SPACECRAFT_ID,
                osdlp::tf_source_destination_id::SOURCE_TRANSFER_FRAME, 0, 0,
                osdlp::tf_bypass_sequence_control_flag::SEQUENCE_CONTROLLED_QOS,
                0, osdlp::tf_operational_control_field_flag::OCF_NOT_PRESENT,
                0);
 
            osdlp::tf_data_field_header uslp_resp_tfdf_hdr;
            osdlp::uslp uslp_resp(uslp_resp_tf_hdr, uslp_resp_tfdf_hdr);
 
            /* Serialize the USLP inside a msg_arbiter::msg */
            auto uslp_len = uslp_resp.serialize(arb.m_msg.data, CONFIG_MAX_MTU,
                                                response.data, response.len);
            arb.m_msg.len = uslp_len;
            arb.m_msg.iface = response.iface;
            arb.fwd(arb.m_msg, arb.m_msg.iface);
          } else {
            /* Try with the mission specific parser */
            mission::get_instance().msg_parser(space_packet, true);
          }
        } else {
          /* Try with the mission specific parser */
          mission::get_instance().msg_parser(arb.m_msg, false);
        }
      }
    } catch (const scl::exception &e) {
      auto &err = error_handler::get_instance();
      err.handle(e);
    } catch (const std::exception &e) {
      auto &err = error_handler::get_instance();
      err.handle(e);
    }
  }
}
 
msg_arbiter::msg_arbiter()
{
  k_msgq_init(&rx_msgq, rx_msgq_buffer, sizeof(msg_arbiter::msg), rx_msgq_size);
  k_msgq_init(&can1_tx_msgq, can1_tx_msgq_buffer, sizeof(msg_arbiter::msg),
              can1_tx_msgq_size);
  k_msgq_init(&can2_tx_msgq, can2_tx_msgq_buffer, sizeof(msg_arbiter::msg),
              can2_tx_msgq_size);
  k_msgq_init(&uhf_tx_msgq, uhf_tx_msgq_buffer, sizeof(msg_arbiter::msg),
              uhf_tx_msgq_size);
  k_msgq_init(&sband_tx_msgq, sband_tx_msgq_buffer, sizeof(msg_arbiter::msg),
              sband_tx_msgq_size);
  k_mutex_init(&m_mtx);
}
 
} // namespace satnogs::comms