VILLASnode/lib/nodes/opal.cpp

/* Node type: OPAL (AsyncApi).
 *
 * This file implements the opal subtype for nodes.
 *
 * Author: Steffen Vogel <post@steffenvogel.de>
 * SPDX-FileCopyrightText: 2014-2023 Institute for Automation of Complex Power Systems, RWTH Aachen University
 * SPDX-License-Identifier: Apache-2.0
 */

#include <cstdlib>
#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>

#include <villas/exceptions.hpp>
#include <villas/node_compat.hpp>
#include <villas/nodes/opal.hpp>
#include <villas/utils.hpp>

extern "C" {
/* Define RTLAB before including OpalPrint.h for messages to be sent
	* to the OpalDisplay. Otherwise stdout will be used. */
#define RTLAB
#include <AsyncApi.h>
#include <OpalGenAsyncParamCtrl.h>
#include <OpalPrint.h>
}

// Private static storage
static std::string
    asyncShmemName; // Shared Memory identifiers and size, provided via argv.
static std::string
    printShmemName; // Shared Memory identifiers and size, provided via argv.
static size_t
    asyncShmemSize; // Shared Memory identifiers and size, provided via argv.

static std::vector<unsigned> sendIDs,
    recvIDs; // A dynamically allocated array of SendIDs.

static Opal_GenAsyncParam_Ctrl
    params; // String and Float parameters, provided by the OPAL AsyncProcess block.

static pthread_mutex_t lock; // Big Global Lock for libOpalAsync API

using namespace villas;
using namespace villas::utils;

/* A bunch of symbols which are used by the libOpal libraries
 * and undefined by GCC. We replace them by GCC variants here.
 */
extern "C" {
int __xstat(int ver, const char *path, struct stat *stat_buf) {
  return stat(path, stat_buf);
}

int backtrace(void **buffer, int size) { return 0; }

char **backtrace_symbols(void *const *buffer, int size) { return nullptr; }

void backtrace_symbols_fd(void *const *buffer, int size, int fd) {}

void *_intel_fast_memset(void *b, int c, size_t len) {
  return memset(b, c, len);
}

void *_intel_fast_memcpy(void *dst, const void *src, size_t n) {
  return memcpy(dst, src, n);
}

int _intel_fast_memcmp(const void *s1, const void *s2, size_t n) {
  return memcmp(s1, s2, n);
}

void *_intel_fast_memmove(void *s1, const void *s2, size_t n) {
  return memmove(s1, s2, n);
}
}

static int opal_register_region(int argc, char *argv[]) {
  if (argc != 4)
    return -1;

  asyncShmemName = argv[1];
  asyncShmemSize = atoi(argv[2]);
  printShmemName = argv[3];

  return 0;
}

int villas::node::opal_type_start(villas::node::SuperNode *sn) {
  int err, noRecvIcons, noSendIcons;

  pthread_mutex_init(&lock, nullptr);

  // Enable the OpalPrint function. This prints to the OpalDisplay.
  err = OpalSystemCtrl_Register((char *)printShmemName.c_str());
  if (err != EOK)
    throw RuntimeError("OpalPrint() access not available ({})", err);

  // Open Share Memory created by the model.
  err = OpalOpenAsyncMem(asyncShmemSize, asyncShmemName.c_str());
  if (err != EOK)
    throw RuntimeError("Model shared memory not available ({})", err);

  err = OpalGetAsyncCtrlParameters(&params, sizeof(Opal_GenAsyncParam_Ctrl));
  if (err != EOK)
    throw RuntimeError("Could not get OPAL controller parameters ({})", err);

  // Get list of Send and RecvIDs
  err = OpalGetNbAsyncSendIcon(&noSendIcons);
  if (err != EOK)
    throw RuntimeError("Failed to get number of send blocks ({})", err);
  err = OpalGetNbAsyncRecvIcon(&noRecvIcons);
  if (err != EOK)
    throw RuntimeError("Failed to get number of recv blocks ({})", err);

  sendIDs.resize(noSendIcons);
  recvIDs.resize(noRecvIcons);

  err = OpalGetAsyncSendIDList(sendIDs.data(), noSendIcons * sizeof(int));
  if (err != EOK)
    throw RuntimeError("Failed to get list of send ids ({})", err);
  err = OpalGetAsyncRecvIDList(recvIDs.data(), noRecvIcons * sizeof(int));
  if (err != EOK)
    throw RuntimeError("Failed to get list of recv ids ({})", err);

  auto logger = Log::get("node:opal");
  logger->info("Started as OPAL Asynchronous process");
  logger->info("This is VILLASnode %s (built on %s, %s)", PROJECT_BUILD_ID,
               __DATE__, __TIME__);

  opal_print_global();

  return 0;
}

int villas::node::opal_type_stop() {
  int err;

  err = OpalCloseAsyncMem(asyncShmemSize, asyncShmemName.c_str());
  if (err != EOK)
    throw RuntimeError("Failed to close shared memory area ({})", err);

  auto logger = Log::get("node:opal");
  logger->debug("Closing OPAL shared memory mapping");

  err = OpalSystemCtrl_UnRegister((char *)printShmemName.c_str());
  if (err != EOK)
    throw RuntimeError("Failed to close shared memory for system control ({})",
                       err);

  pthread_mutex_destroy(&lock);

  return 0;
}

static int opal_print_global() {
  auto logger = Log::get("node:opal");
  logger->debug("Controller ID: {}", params.controllerID);

  std::stringstream sss, rss;

  for (auto i : sendIDs)
    sss << i << " ";
  for (auto i : recvIDs)
    rss << i << " ";

  logger->debug("Send Blocks: {}", sss.str());
  logger->debug("Receive Blocks: {}", rss.str());

  logger->debug("Control Block Parameters:");
  for (int i = 0; i < GENASYNC_NB_FLOAT_PARAM; i++)
    logger->debug("FloatParam[{}] = {}", i, (double)params.FloatParam[i]);
  for (int i = 0; i < GENASYNC_NB_STRING_PARAM; i++)
    logger->debug("StringParam[{}] = {}", i, params.StringParam[i]);

  return 0;
}

int villas::node::opal_parse(NodeCompat *n, json_t *json) {
  auto *o = n->getData<struct opal>();

  int ret;
  json_error_t err;

  ret = json_unpack_ex(json, &err, 0, "{ s: i, s: i, s: b }", "send_id",
                       &o->sendID, "recv_id", &o->sendID, "reply", &o->reply);
  if (ret)
    throw ConfigError(json, err, "node-config-node-opal");

  return 0;
}

char *villas::node::opal_print(NodeCompat *n) {
  auto *o = n->getData<struct opal>();

  // TODO: Print send_params, recv_params

  return strf("sendID=%u, recvID=%u, reply=%u", o->sendID, o->recvID, o->reply);
}

int villas::node::opal_start(NodeCompat *n) {
  auto *o = n->getData<struct opal>();

  // Search for valid send and recv ids
  int sfound = 0, rfound = 0;
  for (auto i : sendIDs)
    sfound += i == o->sendID;
  for (auto i : recvIDs)
    rfound += i == o->sendID;

  if (!sfound)
    throw RuntimeError("Invalid send_id '{}'", o->sendID);
  if (!rfound)
    throw RuntimeError("Invalid recv_id '{}'", o->recvID);

  // Get some more informations and paramters from OPAL-RT
  OpalGetAsyncSendIconMode(&o->mode, o->sendID);
  OpalGetAsyncSendParameters(&o->sendParams, sizeof(Opal_SendAsyncParam),
                             o->sendID);
  OpalGetAsyncRecvParameters(&o->recvParams, sizeof(Opal_RecvAsyncParam),
                             o->recvID);

  o->sequenceNo = 0;

  return 0;
}

int villas::node::opal_read(NodeCompat *n, struct Sample *const smps[],
                            unsigned cnt) {
  auto *o = n->getData<struct opal>();

  int state, ret, len;
  unsigned id;

  struct Sample *s = smps[0];

  double data[s->capacity];

  if (cnt != 1)
    throw RuntimeError("The OPAL-RT node type does not support combining!");

  // This call unblocks when the 'Data Ready' line of a send icon is asserted.
  do {
    ret = OpalWaitForAsyncSendRequest(&id);
    if (ret != EOK) {
      state = OpalGetAsyncModelState();
      if ((state == STATE_RESET) || (state == STATE_STOP))
        throw RuntimeError(
            "OpalGetAsyncModelState(): Model stopped or resetted!");

      return -1; // TODO: correct return value
    }
  } while (id != o->sendID);

  // No errors encountered yet
  OpalSetAsyncSendIconError(0, o->sendID);

  // Get the size of the data being sent by the unblocking SendID
  OpalGetAsyncSendIconDataLength(&len, o->sendID);
  if ((unsigned)len > s->capacity * sizeof(s->data[0])) {
    n->logger->warn("Ignoring the last {} of {} values for OPAL (send_id={}).",
                    len / sizeof(double) - s->capacity, len / sizeof(double),
                    o->sendID);

    len = sizeof(data);
  }

  // Read data from the model
  OpalGetAsyncSendIconData(data, len, o->sendID);

  s->sequence = htons(o->sequenceNo++);
  s->length = (unsigned)len / sizeof(double);

  for (unsigned i = 0; i < s->length; i++)
    s->data[i].f = (float)data[i]; // OPAL provides double precission

  /* This next call allows the execution of the "asynchronous" process
   * to actually be synchronous with the model. To achieve this, you
   * should set the "Sending Mode" in the Async_Send block to
   * NEED_REPLY_BEFORE_NEXT_SEND or NEED_REPLY_NOW. This will force
   * the model to wait for this process to call this
   * OpalAsyncSendRequestDone function before continuing. */
  if (o->reply)
    OpalAsyncSendRequestDone(o->sendID);

  /* Before continuing, we make sure that the real-time model
   * has not been stopped. If it has, we quit. */
  state = OpalGetAsyncModelState();
  if ((state == STATE_RESET) || (state == STATE_STOP))
    throw RuntimeError("OpalGetAsyncModelState(): Model stopped or resetted!");

  return 1;
}

int villas::node::opal_write(NodeCompat *n, struct Sample *const smps[],
                             unsigned cnt) {
  auto *o = n->getData<struct opal>();

  struct Sample *s = smps[0];

  int state;
  int len;
  double data[s->length];

  if (cnt != 1)
    throw RuntimeError("The OPAL-RT node type does not support combining!");

  state = OpalGetAsyncModelState();
  if ((state == STATE_RESET) || (state == STATE_STOP))
    throw RuntimeError("OpalGetAsyncModelState(): Model stopped or resetted!");

  OpalSetAsyncRecvIconStatus(s->sequence,
                             o->recvID);   // Set the Status to the message ID
  OpalSetAsyncRecvIconError(0, o->recvID); // Set the Error to 0

  // Get the number of signals to send back to the model
  OpalGetAsyncRecvIconDataLength(&len, o->recvID);
  if (len > (int)sizeof(data))
    n->logger->warn(
        "Node expecting more signals ({}) than values in message ({})",
        len / sizeof(double), s->length);

  for (unsigned i = 0; i < s->length; i++)
    data[i] = (double)s->data[i].f; // OPAL expects double precission

  OpalSetAsyncRecvIconData(data, s->length * sizeof(double), o->recvID);

  return 1;
}

static NodeCompatType p;

__attribute__((constructor(110))) static void register_plugin() {
  p.name = "opal";
  p.description = "run as OPAL Asynchronous Process (libOpalAsyncApi)";
  p.vectorize = 1;
  p.size = sizeof(struct opal);
  p.type.start = opal_type_start;
  p.type.stop = opal_type_stop;
  p.parse = opal_parse;
  p.print = opal_print;
  p.start = opal_start;
  p.read = opal_read;
  p.write = opal_write;

  static NodeCompatFactory ncp(&p);
}