VILLASnode/lib/nodes/iec61850.cpp

/* Node type: IEC 61850-9-2 (Sampled Values).
 *
 * 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 <cstring>
#include <pthread.h>
#include <unistd.h>

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

#define CONFIG_SV_DEFAULT_APPID 0x4000
#define CONFIG_SV_DEFAULT_DST_ADDRESS CONFIG_GOOSE_DEFAULT_DST_ADDRESS
#define CONFIG_SV_DEFAULT_PRIORITY 4
#define CONFIG_SV_DEFAULT_VLAN_ID 0

using namespace villas;
using namespace villas::node;
using namespace villas::utils;

const struct iec61850_type_descriptor type_descriptors[] = {
    // name,              iec_type,                         type,                 size, supported
    {"boolean", IEC61850Type::BOOLEAN, SignalType::BOOLEAN, 1, false, false},
    {"int8", IEC61850Type::INT8, SignalType::INTEGER, 1, false, false},
    {"int16", IEC61850Type::INT16, SignalType::INTEGER, 2, false, false},
    {"int32", IEC61850Type::INT32, SignalType::INTEGER, 4, false, false},
    {"int64", IEC61850Type::INT64, SignalType::INTEGER, 8, false, false},
    {"int8u", IEC61850Type::INT8U, SignalType::INTEGER, 1, false, false},
    {"int16u", IEC61850Type::INT16U, SignalType::INTEGER, 2, false, false},
    {"int32u", IEC61850Type::INT32U, SignalType::INTEGER, 4, false, false},
    {"int64u", IEC61850Type::INT64U, SignalType::INTEGER, 8, false, false},
    {"float32", IEC61850Type::FLOAT32, SignalType::FLOAT, 4, false, false},
    {"float64", IEC61850Type::FLOAT64, SignalType::FLOAT, 8, false, false},
    {"enumerated", IEC61850Type::ENUMERATED, SignalType::INVALID, 4, false,
     false},
    {"coded_enum", IEC61850Type::CODED_ENUM, SignalType::INVALID, 4, false,
     false},
    {"octet_string", IEC61850Type::OCTET_STRING, SignalType::INVALID, 20, false,
     false},
    {"visible_string", IEC61850Type::VISIBLE_STRING, SignalType::INVALID, 35,
     false, false},
    {"objectname", IEC61850Type::OBJECTNAME, SignalType::INVALID, 20, false,
     false},
    {"objectreference", IEC61850Type::OBJECTREFERENCE, SignalType::INVALID, 20,
     false, false},
    {"timestamp", IEC61850Type::TIMESTAMP, SignalType::INVALID, 8, false,
     false},
    {"entrytime", IEC61850Type::ENTRYTIME, SignalType::INVALID, 6, false,
     false},
    {"bitstring", IEC61850Type::BITSTRING, SignalType::INVALID, 4, false,
     false}};

// Each network interface needs a separate receiver
static struct List receivers;
static pthread_t thread;
static EthernetHandleSet hset;
static int users = 0;

static void *iec61850_thread(void *ctx) {
  int ret;

  while (1) {
    ret = EthernetHandleSet_waitReady(hset, 1000);
    if (ret < 0)
      continue;

    for (unsigned i = 0; i < list_length(&receivers); i++) {
      struct iec61850_receiver *r =
          (struct iec61850_receiver *)list_at(&receivers, i);

      switch (r->type) {
      case iec61850_receiver::Type::GOOSE:
        GooseReceiver_tick(r->goose);
        break;
      case iec61850_receiver::Type::SAMPLED_VALUES:
        SVReceiver_tick(r->sv);
        break;
      }
    }
  }

  return nullptr;
}

const struct iec61850_type_descriptor *
villas::node::iec61850_lookup_type(const char *name) {
  for (unsigned i = 0; i < ARRAY_LEN(type_descriptors); i++) {
    if (!strcmp(name, type_descriptors[i].name))
      return &type_descriptors[i];
  }

  return nullptr;
}

int villas::node::iec61850_parse_signals(json_t *json_signals,
                                         struct List *signals,
                                         SignalList::Ptr node_signals) {
  int ret, total_size = 0;
  const char *iec_type;
  const struct iec61850_type_descriptor *td;
  json_error_t err;

  ret = list_init(signals);
  if (ret)
    return ret;

  if (json_is_array(json_signals)) {
    json_t *json_signal;
    size_t i;
    json_array_foreach(json_signals, i, json_signal) {
      json_unpack_ex(json_signal, &err, 0, "{ s?: s }", "iec_type", &iec_type);

      // Try to deduct the IEC 61850 data type from VILLAS signal format
      if (!iec_type) {
        if (!node_signals)
          return -1;

        auto sig = node_signals->getByIndex(i);
        if (!sig)
          return -1;

        switch (sig->type) {
        case SignalType::BOOLEAN:
          iec_type = "boolean";
          break;

        case SignalType::FLOAT:
          iec_type = "float64";
          break;

        case SignalType::INTEGER:
          iec_type = "int64";
          break;

        default:
          return -1;
        }
      }

      td = iec61850_lookup_type(iec_type);
      if (!td)
        return -1;

      list_push(signals, (void *)td);

      total_size += td->size;
    }
  } else {
    ret = json_unpack_ex(json_signals, &err, 0, "{ s: s }", "iec_type",
                         &iec_type);
    if (ret)
      return ret;

    td = iec61850_lookup_type(iec_type);
    if (!td)
      return -1;

    for (unsigned i = 0; i < node_signals->size(); i++) {
      list_push(signals, (void *)td);

      total_size += td->size;
    }
  }

  return total_size;
}

int villas::node::iec61850_type_start(villas::node::SuperNode *sn) {
  int ret;

  // Check if already initialized
  if (users > 0)
    return 0;

  ret = list_init(&receivers);
  if (ret)
    return ret;

  hset = EthernetHandleSet_new();

  ret = pthread_create(&thread, nullptr, iec61850_thread, nullptr);
  if (ret)
    return ret;

  return 0;
}

int villas::node::iec61850_type_stop() {
  int ret;

  if (--users > 0)
    return 0;

  for (unsigned i = 0; i < list_length(&receivers); i++) {
    struct iec61850_receiver *r =
        (struct iec61850_receiver *)list_at(&receivers, i);

    iec61850_receiver_stop(r);
  }

  ret = pthread_cancel(thread);
  if (ret)
    return ret;

  ret = pthread_join(thread, nullptr);
  if (ret)
    return ret;

  EthernetHandleSet_destroy(hset);

  ret = list_destroy(&receivers, (dtor_cb_t)iec61850_receiver_destroy, true);
  if (ret)
    return ret;

  return 0;
}

int villas::node::iec61850_receiver_start(struct iec61850_receiver *r) {
  switch (r->type) {
  case iec61850_receiver::Type::GOOSE:
    r->socket = GooseReceiver_startThreadless(r->goose);
    break;

  case iec61850_receiver::Type::SAMPLED_VALUES:
    r->socket = SVReceiver_startThreadless(r->sv);
    break;
  }

  EthernetHandleSet_addSocket(hset, r->socket);

  return 0;
}

int villas::node::iec61850_receiver_stop(struct iec61850_receiver *r) {
  EthernetHandleSet_removeSocket(hset, r->socket);

  switch (r->type) {
  case iec61850_receiver::Type::GOOSE:
    GooseReceiver_stopThreadless(r->goose);
    break;

  case iec61850_receiver::Type::SAMPLED_VALUES:
    SVReceiver_stopThreadless(r->sv);
    break;
  }

  return 0;
}

int villas::node::iec61850_receiver_destroy(struct iec61850_receiver *r) {
  switch (r->type) {
  case iec61850_receiver::Type::GOOSE:
    GooseReceiver_destroy(r->goose);
    break;

  case iec61850_receiver::Type::SAMPLED_VALUES:
    SVReceiver_destroy(r->sv);
    break;
  }

  free(r->interface);

  return 0;
}

struct iec61850_receiver *
villas::node::iec61850_receiver_lookup(enum iec61850_receiver::Type t,
                                       const char *intf) {
  for (unsigned i = 0; i < list_length(&receivers); i++) {
    struct iec61850_receiver *r =
        (struct iec61850_receiver *)list_at(&receivers, i);

    if (r->type == t && strcmp(r->interface, intf) == 0)
      return r;
  }

  return nullptr;
}

struct iec61850_receiver *
villas::node::iec61850_receiver_create(enum iec61850_receiver::Type t,
                                       const char *intf) {
  struct iec61850_receiver *r;

  // Check if there is already a SVReceiver for this interface
  r = iec61850_receiver_lookup(t, intf);
  if (!r) {
    r = new struct iec61850_receiver;
    if (!r)
      throw MemoryAllocationError();

    r->interface = strdup(intf);
    r->type = t;

    switch (r->type) {
    case iec61850_receiver::Type::GOOSE:
      r->goose = GooseReceiver_create();
      GooseReceiver_setInterfaceId(r->goose, r->interface);
      break;

    case iec61850_receiver::Type::SAMPLED_VALUES:
      r->sv = SVReceiver_create();
      SVReceiver_setInterfaceId(r->sv, r->interface);
      break;
    }

    iec61850_receiver_start(r);

    list_push(&receivers, r);
  }

  return r;
}