secs-gem/src/secs2/codec.cpp

#include "secsgem/secs2/codec.hpp"

#include <bit>
#include <cstdio>
#include <type_traits>

namespace secsgem::secs2 {
namespace {

template <typename T>
void put_scalar_be(std::vector<uint8_t>& out, T value) {
  if constexpr (std::is_same_v<T, float>) {
    uint32_t bits = std::bit_cast<uint32_t>(value);
    for (int i = 3; i >= 0; --i) out.push_back(static_cast<uint8_t>(bits >> (8 * i)));
  } else if constexpr (std::is_same_v<T, double>) {
    uint64_t bits = std::bit_cast<uint64_t>(value);
    for (int i = 7; i >= 0; --i) out.push_back(static_cast<uint8_t>(bits >> (8 * i)));
  } else {
    using U = std::make_unsigned_t<T>;
    U u = static_cast<U>(value);
    for (int i = static_cast<int>(sizeof(T)) - 1; i >= 0; --i)
      out.push_back(static_cast<uint8_t>(u >> (8 * i)));
  }
}

template <typename T>
T get_scalar_be(const uint8_t* p) {
  if constexpr (std::is_same_v<T, float>) {
    uint32_t bits = 0;
    for (int i = 0; i < 4; ++i) bits = (bits << 8) | p[i];
    return std::bit_cast<float>(bits);
  } else if constexpr (std::is_same_v<T, double>) {
    uint64_t bits = 0;
    for (int i = 0; i < 8; ++i) bits = (bits << 8) | p[i];
    return std::bit_cast<double>(bits);
  } else {
    using U = std::make_unsigned_t<T>;
    U u = 0;
    for (std::size_t i = 0; i < sizeof(T); ++i) u = static_cast<U>((u << 8) | p[i]);
    return static_cast<T>(u);
  }
}

template <typename T>
std::vector<T> read_array(const uint8_t* p, std::size_t bytes) {
  if (bytes % sizeof(T) != 0)
    throw CodecError("item byte length is not a multiple of the element size");
  const std::size_t n = bytes / sizeof(T);
  std::vector<T> out;
  out.reserve(n);
  for (std::size_t i = 0; i < n; ++i) out.push_back(get_scalar_be<T>(p + i * sizeof(T)));
  return out;
}

void write_header(std::vector<uint8_t>& out, Format fmt, std::size_t length) {
  std::size_t nlen;
  if (length <= 0xFF) nlen = 1;
  else if (length <= 0xFFFF) nlen = 2;
  else if (length <= 0xFFFFFF) nlen = 3;
  else throw CodecError("item length exceeds 3-byte maximum");

  out.push_back(static_cast<uint8_t>((static_cast<uint8_t>(fmt) << 2) | nlen));
  for (std::size_t i = 0; i < nlen; ++i) {
    const std::size_t shift = 8 * (nlen - 1 - i);
    out.push_back(static_cast<uint8_t>((length >> shift) & 0xFF));
  }
}

}  // namespace

void encode_into(const Item& item, std::vector<uint8_t>& out) {
  const Format fmt = item.format();

  if (fmt == Format::List) {
    const auto& children = item.as_list();
    write_header(out, fmt, children.size());
    for (const auto& child : children) encode_into(child, out);
    return;
  }

  std::visit(
      [&](const auto& v) {
        using V = std::decay_t<decltype(v)>;
        if constexpr (std::is_same_v<V, Item::List>) {
          // unreachable: lists handled above
        } else if constexpr (std::is_same_v<V, std::string>) {
          write_header(out, fmt, v.size());
          out.insert(out.end(), v.begin(), v.end());
        } else {
          using Elem = typename V::value_type;
          write_header(out, fmt, v.size() * sizeof(Elem));
          for (auto e : v) put_scalar_be(out, e);
        }
      },
      item.storage());
}

std::vector<uint8_t> encode(const Item& item) {
  std::vector<uint8_t> out;
  encode_into(item, out);
  return out;
}

Item decode_at(const uint8_t* data, std::size_t len, std::size_t& pos) {
  if (pos >= len) throw CodecError("unexpected end of input reading format byte");

  const uint8_t format_byte = data[pos++];
  const uint8_t nlen = format_byte & 0x03;
  const Format fmt = static_cast<Format>((format_byte >> 2) & 0x3F);

  if (nlen == 0) throw CodecError("invalid item: zero length bytes");
  if (pos + nlen > len) throw CodecError("unexpected end of input reading length bytes");

  std::size_t length = 0;
  for (uint8_t i = 0; i < nlen; ++i) length = (length << 8) | data[pos++];

  if (fmt == Format::List) {
    Item::List items;
    items.reserve(length);
    for (std::size_t i = 0; i < length; ++i) items.push_back(decode_at(data, len, pos));
    return Item::list(std::move(items));
  }

  if (pos + length > len) throw CodecError("unexpected end of input reading item data");
  const uint8_t* p = data + pos;
  pos += length;

  switch (fmt) {
    case Format::ASCII:
      return Item::ascii(std::string(reinterpret_cast<const char*>(p), length));
    case Format::JIS8:
      return Item::jis8(std::string(reinterpret_cast<const char*>(p), length));
    case Format::C2:
      return Item::c2(read_array<uint16_t>(p, length));
    case Format::Binary:
      return Item::binary(std::vector<uint8_t>(p, p + length));
    case Format::Boolean:
      return Item::boolean(std::vector<uint8_t>(p, p + length));
    case Format::U1:
      return Item::u1(std::vector<uint8_t>(p, p + length));
    case Format::I1:
      return Item::i1(read_array<int8_t>(p, length));
    case Format::U2:
      return Item::u2(read_array<uint16_t>(p, length));
    case Format::I2:
      return Item::i2(read_array<int16_t>(p, length));
    case Format::U4:
      return Item::u4(read_array<uint32_t>(p, length));
    case Format::I4:
      return Item::i4(read_array<int32_t>(p, length));
    case Format::F4:
      return Item::f4(read_array<float>(p, length));
    case Format::U8:
      return Item::u8(read_array<uint64_t>(p, length));
    case Format::I8:
      return Item::i8(read_array<int64_t>(p, length));
    case Format::F8:
      return Item::f8(read_array<double>(p, length));
    case Format::List:
      break;  // handled above
  }
  throw CodecError("unknown SECS-II format code");
}

Item decode(const std::vector<uint8_t>& bytes) {
  std::size_t pos = 0;
  Item item = decode_at(bytes.data(), bytes.size(), pos);
  if (pos != bytes.size()) throw CodecError("trailing bytes after decoded item");
  return item;
}

namespace {

void sml_into(const Item& item, std::string& out) {
  const Format fmt = item.format();
  out += '<';
  out += format_name(fmt);

  if (fmt == Format::List) {
    out += " [" + std::to_string(item.size()) + "]";
    for (const auto& child : item.as_list()) {
      out += ' ';
      sml_into(child, out);
    }
    out += " >";
    return;
  }

  std::visit(
      [&](const auto& v) {
        using V = std::decay_t<decltype(v)>;
        if constexpr (std::is_same_v<V, Item::List>) {
          // unreachable
        } else if constexpr (std::is_same_v<V, std::string>) {
          out += " \"" + v + "\"";
        } else {
          using Elem = typename V::value_type;
          for (auto e : v) {
            out += ' ';
            if constexpr (std::is_same_v<Elem, uint8_t>) {
              if (fmt == Format::Boolean) {
                out += (e ? "T" : "F");
              } else {
                char buf[5];
                std::snprintf(buf, sizeof(buf), "0x%02X", e);
                out += buf;
              }
            } else if constexpr (std::is_floating_point_v<Elem>) {
              out += std::to_string(e);
            } else {
              out += std::to_string(e);
            }
          }
        }
      },
      item.storage());

  out += " >";
}

}  // namespace

std::string to_sml(const Item& item) {
  std::string out;
  sml_into(item, out);
  return out;
}

}  // namespace secsgem::secs2