io.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 "io.hpp"
#include <dsp/ccsds.hpp>
#include <etl/crc.h>
 
namespace satnogs::comms
{
 
static constexpr bool pc104_uart_en =
    DT_NODE_HAS_STATUS(DT_NODELABEL(usart3), okay);
 
static struct k_thread radio_rx_thread_data;
K_THREAD_STACK_DEFINE(radio_rx_thread_stack, CONFIG_RADIO_RX_THREAD_STACK_SIZE);
static struct k_thread uhf_tx_thread_data;
K_THREAD_STACK_DEFINE(uhf_tx_thread_stack, CONFIG_UHF_TX_THREAD_STACK_SIZE);
static struct k_thread sband_tx_thread_data;
K_THREAD_STACK_DEFINE(sband_tx_thread_stack, CONFIG_SBAND_TX_THREAD_STACK_SIZE);
 
#if CONFIG_IO_CAN1_ENABLE
static const struct device *can1_dev = DEVICE_DT_GET(DT_NODELABEL(fdcan1));
 
static struct k_thread can1_tx_thread_data;
static struct k_thread can1_rx_thread_data;
K_THREAD_STACK_DEFINE(can1_tx_thread_stack, CONFIG_CAN1_TX_THREAD_STACK_SIZE);
K_THREAD_STACK_DEFINE(can1_rx_thread_stack, CONFIG_CAN1_RX_THREAD_STACK_SIZE);
 
/* We want to accept messages from everyone */
static const struct isotp_msg_id can1_rx_filter = {
    .std_id   = CONFIG_CAN1_ISOTP_REMOTE_PEER_RX_ADDR,
    .ext_addr = 0,
    .dl       = 8,
    .flags    = 0};
 
static const struct isotp_msg_id can1_tx_conf = {
    .std_id = CONFIG_CAN1_ISOTP_TX_ADDR, .ext_addr = 0, .dl = 8, .flags = 0};
static const struct isotp_msg_id can1_tx_remote_conf = {
    .std_id   = CONFIG_CAN1_ISOTP_REMOTE_PEER_TX_ADDR,
    .ext_addr = 0,
    .dl       = 8,
    .flags    = 0};
 
static const struct isotp_msg_id can1_rx_conf = {
    .std_id = CONFIG_CAN1_ISOTP_RX_ADDR, .ext_addr = 0, .dl = 8, .flags = 0};
/*
 * Set the BS to 0 for receiving the ISOTP frame in a single isotp_recv_net()
 * call
 */
const struct isotp_fc_opts can1_fc_opts = {.bs = 8, .stmin = 0};
 
static struct isotp_recv_ctx can1_recv_ctx;
static struct isotp_send_ctx can1_send_ctx;
 
#endif
 
#if CONFIG_IO_CAN2_ENABLE
static const struct device *can2_dev = DEVICE_DT_GET(DT_NODELABEL(fdcan2));
 
static struct k_thread can2_tx_thread_data;
static struct k_thread can2_rx_thread_data;
K_THREAD_STACK_DEFINE(can2_tx_thread_stack, CONFIG_CAN2_TX_THREAD_STACK_SIZE);
K_THREAD_STACK_DEFINE(can2_rx_thread_stack, CONFIG_CAN2_RX_THREAD_STACK_SIZE);
 
static const struct isotp_msg_id can2_rx_filter = {
    .std_id   = CONFIG_CAN2_ISOTP_REMOTE_PEER_RX_ADDR,
    .ext_addr = 0,
    .dl       = 8,
    .flags    = 0};
 
static const struct isotp_msg_id can2_tx_conf = {
    .std_id = CONFIG_CAN2_ISOTP_TX_ADDR, .ext_addr = 0, .dl = 8, .flags = 0};
static const struct isotp_msg_id can2_tx_remote_conf = {
    .std_id   = CONFIG_CAN2_ISOTP_REMOTE_PEER_TX_ADDR,
    .ext_addr = 0,
    .dl       = 8,
    .flags    = 0};
 
static const struct isotp_msg_id can2_rx_conf = {
    .std_id = CONFIG_CAN2_ISOTP_RX_ADDR, .ext_addr = 0, .dl = 8, .flags = 0};
/*
 * Set the BS to 0 for receiving the ISOTP frame in a single isotp_recv_net()
 * call
 */
const struct isotp_fc_opts can2_fc_opts = {.bs = 8, .stmin = 0};
 
static struct isotp_recv_ctx can2_recv_ctx;
static struct isotp_send_ctx can2_send_ctx;
 
#endif
/*
 * Those messages are quite large and we do not want to take the memory from the
 * task stack
 */
#if CONFIG_IO_CAN1_ENABLE
static msg_arbiter::msg can1_tx_msg;
static msg_arbiter::msg can1_rx_msg;
#endif
 
#if CONFIG_IO_CAN2_ENABLE
static msg_arbiter::msg can2_tx_msg;
static msg_arbiter::msg can2_rx_msg;
#endif
 
static scl::radio::rx_msg radio_rx_msg;
static msg_arbiter::msg   radio_msg_to_arb;
static msg_arbiter::msg   uhf_msg_from_arb;
static msg_arbiter::msg   sband_msg_from_arb;
 
#if DT_NODE_HAS_STATUS(DT_ALIAS(pc104_usart3), okay)
static void
uart_handling_callback(const struct device *dev, struct uart_event *evt,
                       void *user_data);
#endif
 
#if DT_NODE_HAS_STATUS(DT_ALIAS(pc104_usart3), okay)
io::uart usart3(io::uart::identifier::PC104);
#endif
 
io::radio &
io::radio_ctrl()
{
  return m_radio;
}
 
void
io::start()
{
  /* Register necessary callbacks */
  auto drop_clbk = etl::delegate<void(scl::radio::interface)>::create<
      io, &io::radio_rx_drop_callback>(io::get_instance());
  scl::board::get_instance().radio().register_rx_drop_callback(drop_clbk);
 
  auto &err = error_handler::get_instance();
  auto &pwr = scl::board::get_instance().power();
  int   ret = 0;
 
  /*
   * By default both radio interfaces are enabled. It's the responsibility of
   * the mission to disable any of them if not needed
   */
  m_radio.enable();
 
#if CONFIG_IO_CAN1_ENABLE
  /* Enable the power line before doing anything */
  pwr.enable(scl::power::subsys::CAN1);
  k_msleep(10);
  ret = can_start(can1_dev);
  err.assert_error<can_enable_exception>((ret == 0), __FILE__, __LINE__);
 
  ret = isotp_bind(&can1_recv_ctx, can1_dev, &can1_rx_filter, &can1_rx_conf,
                   &can1_fc_opts, K_FOREVER);
  err.assert_error<isotp_bind_exception>((ret == 0), __FILE__, __LINE__);
 
  m_can1_tx_tid = k_thread_create(&can1_tx_thread_data, can1_tx_thread_stack,
                                  K_THREAD_STACK_SIZEOF(can1_tx_thread_stack),
                                  can1_tx_thread, NULL, NULL, NULL,
                                  CONFIG_CAN1_TX_THREAD_PRIO, 0, K_NO_WAIT);
  if (!m_can1_tx_tid) {
    k_oops();
  }
  k_thread_name_set(m_can1_tx_tid, "can1_tx");
 
  m_can1_rx_tid = k_thread_create(&can1_rx_thread_data, can1_rx_thread_stack,
                                  K_THREAD_STACK_SIZEOF(can1_rx_thread_stack),
                                  can1_rx_thread, NULL, NULL, NULL,
                                  CONFIG_CAN1_RX_THREAD_PRIO, 0, K_NO_WAIT);
  if (!m_can1_rx_tid) {
    k_oops();
  }
  k_thread_name_set(m_can1_rx_tid, "can1_rx");
#endif
 
#if CONFIG_IO_CAN2_ENABLE
  /* Enable the power line before doing anything */
  pwr.enable(scl::power::subsys::CAN2);
  k_msleep(10);
  ret = can_start(can2_dev);
  err.assert_error<can_enable_exception>((ret == 0), __FILE__, __LINE__);
 
  ret = isotp_bind(&can2_recv_ctx, can2_dev, &can2_rx_filter, &can2_rx_conf,
                   &can2_fc_opts, K_FOREVER);
  err.assert_error<isotp_bind_exception>((ret == 0), __FILE__, __LINE__);
 
  m_can2_tx_tid = k_thread_create(&can2_tx_thread_data, can2_tx_thread_stack,
                                  K_THREAD_STACK_SIZEOF(can2_tx_thread_stack),
                                  can2_tx_thread, NULL, NULL, NULL,
                                  CONFIG_CAN2_TX_THREAD_PRIO, 0, K_NO_WAIT);
  if (!m_can2_tx_tid) {
    k_oops();
  }
  k_thread_name_set(m_can2_tx_tid, "can2_tx");
 
  m_can2_rx_tid = k_thread_create(&can2_rx_thread_data, can2_rx_thread_stack,
                                  K_THREAD_STACK_SIZEOF(can2_rx_thread_stack),
                                  can2_rx_thread, NULL, NULL, NULL,
                                  CONFIG_CAN2_RX_THREAD_PRIO, 0, K_NO_WAIT);
  if (!m_can2_rx_tid) {
    k_oops();
  }
  k_thread_name_set(m_can2_rx_tid, "can2_rx");
#endif
 
  m_radio_rx_tid = k_thread_create(&radio_rx_thread_data, radio_rx_thread_stack,
                                   K_THREAD_STACK_SIZEOF(radio_rx_thread_stack),
                                   radio_rx_thread, NULL, NULL, NULL,
                                   CONFIG_RADIO_RX_THREAD_PRIO, 0, K_NO_WAIT);
 
  m_uhf_tx_tid = k_thread_create(&uhf_tx_thread_data, uhf_tx_thread_stack,
                                 K_THREAD_STACK_SIZEOF(uhf_tx_thread_stack),
                                 uhf_tx_thread, NULL, NULL, NULL,
                                 CONFIG_UHF_TX_THREAD_PRIO, 0, K_NO_WAIT);
 
  m_sband_tx_tid = k_thread_create(&sband_tx_thread_data, sband_tx_thread_stack,
                                   K_THREAD_STACK_SIZEOF(sband_tx_thread_stack),
                                   sband_tx_thread, NULL, NULL, NULL,
                                   CONFIG_SBAND_TX_THREAD_PRIO, 0, K_NO_WAIT);
 
  if (!m_radio_rx_tid) {
    k_oops();
  }
  if (!m_uhf_tx_tid) {
    k_oops();
  }
 
  if (!m_sband_tx_tid) {
    k_oops();
  }
  k_thread_name_set(m_radio_rx_tid, "radio_rx");
  k_thread_name_set(m_uhf_tx_tid, "uhf_tx");
  k_thread_name_set(m_sband_tx_tid, "sband_tx");
 
#if DT_NODE_HAS_STATUS(DT_ALIAS(pc104_usart3), okay)
  const struct device *uart_dev = DEVICE_DT_GET(DT_ALIAS(pc104_usart3));
 
  const struct uart_config uart_cfg = {.baudrate  = CONFIG_PC104_UART_BAUDRATE,
                                       .parity    = UART_CFG_PARITY_NONE,
                                       .stop_bits = UART_CFG_STOP_BITS_1,
                                       .data_bits = UART_CFG_DATA_BITS_8,
                                       .flow_ctrl = UART_CFG_FLOW_CTRL_NONE};
  uart_rx_disable(uart_dev);
  uart_configure(uart_dev, &uart_cfg);
  uart_callback_set(uart_dev, uart_handling_callback, &usart3);
  uart_rx_enable(uart_dev, usart3.buf[usart3.buf_idx], io::uart::buf_len,
                 CONFIG_PC104_UART_RX_TIMEOUT_MS);
#endif
}
 
#if CONFIG_IO_CAN1_ENABLE
void
io::can1_tx_thread(void *arg1, void *arg2, void *arg3)
{
  auto        &err         = error_handler::get_instance();
  int          task_wdt_id = task_wdt_add(CONFIG_WATCHDOG_PERIOD_CAN1_TX,
                                          task_wdt_callback, (void *)k_current_get());
  msg_arbiter &arb         = msg_arbiter::get_instance();
  int          ret;
  while (1) {
    try {
      task_wdt_feed(task_wdt_id);
      ret = arb.pull(can1_tx_msg, msg_arbiter::subsys::CAN1, K_MSEC(100));
      if (ret == 0) {
        isotp_send(&can1_send_ctx, can1_dev, can1_tx_msg.data, can1_tx_msg.len,
                   &can1_tx_conf, &can1_tx_remote_conf, NULL, NULL);
      }
    } catch (const scl::exception &e) {
      err.handle(e);
    } catch (const std::exception &e) {
      err.handle(e);
    }
  }
}
 
void
io::can1_rx_thread(void *arg1, void *arg2, void *arg3)
{
  auto           &err         = error_handler::get_instance();
  int             task_wdt_id = task_wdt_add(CONFIG_WATCHDOG_PERIOD_CAN1_RX,
                                             task_wdt_callback, (void *)k_current_get());
  msg_arbiter    &arb         = msg_arbiter::get_instance();
  struct net_buf *buf;
  int             ret;
 
  while (1) {
    try {
      task_wdt_feed(task_wdt_id);
      can1_rx_msg.iface = msg_arbiter::subsys::CAN1;
      can1_rx_msg.len   = 0;
      do {
        ret = isotp_recv_net(&can1_recv_ctx, &buf, K_MSEC(100));
        if (ret == ISOTP_RECV_TIMEOUT) {
          /* This is ok! */
          break;
        }
        if (ret < 0) {
          err.assert_error<isotp_recv_exception>(__FILE__, __LINE__);
          break;
        }
        while (buf != nullptr) {
          /* Clamp to the available MTU */
          size_t ncpy =
              std::min<size_t>(msg_arbiter::mtu - can1_rx_msg.len, buf->len);
 
          std::memcpy(can1_rx_msg.data + can1_rx_msg.len, buf->data, ncpy);
          can1_rx_msg.len += ncpy;
          buf = net_buf_frag_del(NULL, buf);
        }
      } while (ret > 0);
      if (can1_rx_msg.len > 0) {
        arb.push(can1_rx_msg);
      }
    } catch (const scl::exception &e) {
      err.handle(e);
    } catch (const std::exception &e) {
      err.handle(e);
    }
  }
}
 
#endif
 
#if CONFIG_IO_CAN2_ENABLE
void
io::can2_tx_thread(void *arg1, void *arg2, void *arg3)
{
  auto        &err         = error_handler::get_instance();
  int          task_wdt_id = task_wdt_add(CONFIG_WATCHDOG_PERIOD_CAN2_TX,
                                          task_wdt_callback, (void *)k_current_get());
  msg_arbiter &arb         = msg_arbiter::get_instance();
  int          ret;
  while (1) {
    try {
      task_wdt_feed(task_wdt_id);
      ret = arb.pull(can2_tx_msg, msg_arbiter::subsys::CAN2, K_MSEC(100));
      if (ret == 0) {
        isotp_send(&can2_send_ctx, can2_dev, can2_tx_msg.data, can2_tx_msg.len,
                   &can2_tx_conf, &can2_tx_remote_conf, NULL, NULL);
      }
    } catch (const scl::exception &e) {
      err.handle(e);
    } catch (const std::exception &e) {
      err.handle(e);
    }
  }
}
 
void
io::can2_rx_thread(void *arg1, void *arg2, void *arg3)
{
  auto           &err         = error_handler::get_instance();
  int             task_wdt_id = task_wdt_add(CONFIG_WATCHDOG_PERIOD_CAN2_RX,
                                             task_wdt_callback, (void *)k_current_get());
  msg_arbiter    &arb         = msg_arbiter::get_instance();
  struct net_buf *buf;
  int             ret;
 
  while (1) {
    try {
      task_wdt_feed(task_wdt_id);
      can2_rx_msg.iface = msg_arbiter::subsys::CAN2;
      can2_rx_msg.len   = 0;
      do {
        ret = isotp_recv_net(&can2_recv_ctx, &buf, K_MSEC(100));
        if (ret == ISOTP_RECV_TIMEOUT) {
          /* This is ok! */
          break;
        }
        if (ret < 0) {
          err.assert_error<isotp_recv_exception>(__FILE__, __LINE__);
          break;
        }
        while (buf != nullptr) {
          /* Clamp to the available MTU */
          size_t ncpy =
              std::min<size_t>(msg_arbiter::mtu - can2_rx_msg.len, buf->len);
 
          std::memcpy(can2_rx_msg.data + can2_rx_msg.len, buf->data, ncpy);
          can2_rx_msg.len += ncpy;
          buf = net_buf_frag_del(NULL, buf);
        }
      } while (ret > 0);
      if (can2_rx_msg.len > 0) {
        arb.push(can2_rx_msg);
      }
    } catch (const scl::exception &e) {
      err.handle(e);
    } catch (const std::exception &e) {
      err.handle(e);
    }
  }
}
#endif
 
static uint32_t
received_crc(const uint8_t *b, uint32_t len)
{
  return (b[len - sizeof(uint32_t)] << 24) |
         (b[len - sizeof(uint32_t) + 1] << 16) |
         (b[len - sizeof(uint32_t) + 2] << 8) | (b[len - sizeof(uint32_t) + 3]);
}
 
void
io::radio_rx_thread(void *arg1, void *arg2, void *arg3)
{
  int          task_wdt_id = task_wdt_add(CONFIG_WATCHDOG_PERIOD_RADIO_RX,
                                          task_wdt_callback, (void *)k_current_get());
  auto        &radio       = scl::board::get_instance().radio();
  msg_arbiter &arb         = msg_arbiter::get_instance();
  while (1) {
    try {
      task_wdt_feed(task_wdt_id);
      int ret = radio.recv_msg(radio_rx_msg, 1000);
      if (ret == 0) {
        /* Perform descrambling first */
        dsp::ccsds::descrambler(radio_rx_msg.pdu, radio_rx_msg.info.len);
 
        /* Check the validity of the frame */
        auto crc = etl::crc32_c(radio_rx_msg.pdu, radio_rx_msg.pdu +
                                                      radio_rx_msg.info.len -
                                                      sizeof(uint32_t))
                       .value();
        uint32_t recv_crc =
            received_crc(radio_rx_msg.pdu, radio_rx_msg.info.len);
        /* Update the internal statistics of the interface */
        io::get_instance().update_radio_rx_stats(
            radio_rx_msg.info.iface, crc == recv_crc, radio_rx_msg.info.rssi);
 
        /* Forward the frame to the message arbiter only if the CRC was valid */
        if (crc == recv_crc) {
          radio_msg_to_arb.iface =
              radio_rx_msg.info.iface == scl::radio::interface::UHF
                  ? msg_arbiter::subsys::RADIO_UHF
                  : msg_arbiter::subsys::RADIO_SBAND;
          std::copy_n(radio_rx_msg.pdu, radio_rx_msg.info.len,
                      radio_msg_to_arb.data);
 
          radio_msg_to_arb.len = radio_rx_msg.info.len;
          arb.push(radio_msg_to_arb);
        }
      }
    } 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);
    }
  }
}
 
void
io::uhf_tx_thread(void *arg1, void *arg2, void *arg3)
{
  int          task_wdt_id = task_wdt_add(CONFIG_WATCHDOG_PERIOD_RADIO_TX,
                                          task_wdt_callback, (void *)k_current_get());
  auto        &radio_ctrl  = io::get_instance().radio_ctrl();
  msg_arbiter &arb         = msg_arbiter::get_instance();
 
  while (1) {
    try {
      task_wdt_feed(task_wdt_id);
      int ret = arb.pull(uhf_msg_from_arb, msg_arbiter::subsys::RADIO_UHF,
                         K_MSEC(1000));
      // Nothing received probably. Try again
      if (ret) {
        continue;
      }
      radio_ctrl.tx(scl::radio::interface::UHF, uhf_msg_from_arb,
                    arb.backpressure(msg_arbiter::subsys::RADIO_UHF) > 0);
    } 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);
    }
  }
}
 
void
io::sband_tx_thread(void *arg1, void *arg2, void *arg3)
{
  int          task_wdt_id = task_wdt_add(CONFIG_WATCHDOG_PERIOD_RADIO_TX,
                                          task_wdt_callback, (void *)k_current_get());
  auto        &radio_ctrl  = io::get_instance().radio_ctrl();
  msg_arbiter &arb         = msg_arbiter::get_instance();
 
  while (1) {
    try {
      task_wdt_feed(task_wdt_id);
      int ret = arb.pull(sband_msg_from_arb, msg_arbiter::subsys::RADIO_SBAND,
                         K_MSEC(1000));
      if (ret) {
        continue;
      }
      radio_ctrl.tx(scl::radio::interface::SBAND, sband_msg_from_arb,
                    arb.backpressure(msg_arbiter::subsys::RADIO_SBAND) > 0);
    } 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);
    }
  }
}
 
void
io::radio_rx_drop_callback(scl::radio::interface iface)
{
  switch (iface) {
  case scl::radio::interface::UHF:
    m_radio.m_uhf.rx_frame_dropped();
    return;
  case scl::radio::interface::SBAND:
    m_radio.m_sband.rx_frame_dropped();
    return;
  }
}
 
void
io::update_radio_rx_stats(scl::radio::interface iface, bool valid, float rssi)
{
  switch (iface) {
  case scl::radio::interface::UHF:
    m_radio.m_uhf.frame_received(valid, rssi);
    return;
  case scl::radio::interface::SBAND:
    m_radio.m_sband.frame_received(valid, rssi);
    return;
  }
}
 
#if DT_NODE_HAS_STATUS(DT_ALIAS(pc104_usart3), okay)
static void
uart_handling_callback(const struct device *dev, struct uart_event *evt,
                       void *user_data)
{
  io::uart *uart = reinterpret_cast<io::uart *>(user_data);
 
  switch (evt->type) {
  case UART_TX_DONE:
    break;
  case UART_TX_ABORTED:
    break;
  case UART_RX_RDY: {
    /* Ensure no overflow can happen */
    size_t ncopy =
        std::min<size_t>(CONFIG_MAX_MTU - uart->msg_idx, evt->data.rx.len);
    std::copy_n(&evt->data.rx.buf[evt->data.rx.offset], ncopy,
                &uart->msg.data[uart->msg_idx]);
    uart->msg_idx += ncopy;
    msg_arbiter &arb = msg_arbiter::get_instance();
    uart->msg.iface  = msg_arbiter::subsys::UART_PC104;
    uart->msg.len    = uart->msg_idx;
    arb.push(uart->msg);
    uart->msg_idx = 0;
  } break;
  case UART_RX_BUF_RELEASED:
    break;
  case UART_RX_BUF_REQUEST:
    uart->buf_idx = (uart->buf_idx + 1) % 2;
    uart_rx_buf_rsp(dev, uart->buf[uart->buf_idx], io::uart::buf_len);
    break;
  case UART_RX_DISABLED:
    break;
  default:
    break;
  }
}
#endif
 
} // namespace satnogs::comms