fragformat.hpp

Go to the documentation of this file.

// ╭─╮╭─╴╷╭─╮┌─╮
// ╰─╯╵  ╵╰─╯╵ ╵
#pragma once
 
#include <cassert>
#include <vector>
#include <span>
 
#include "itk/logger.hpp"
#include "fragheader.hpp"
 
namespace itk::fragment {
 
extern itk::Logger log;
 
 
FelixPacket make_packet(uint64_t fid, std::span<const uint64_t> data, uint8_t status) {
  FelixPacket pkt;
 
  pkt.fid    = fid;
  pkt.time   = 0;
  pkt.data   = reinterpret_cast<const uint8_t*>(data.data());
  pkt.size   = data.size()*8;
  pkt.status = status;
 
  return pkt;
}
 
 
namespace ROD { // Read-Out Driver
 
 struct PayloadData {
  std::vector<uint32_t> data;
  std::vector<uint32_t> status;
 
  inline uint32_t get_data_size() { return data.size(); }
 
  inline uint32_t get_status_size() { return status.size(); }
 
  template<typename T>
  void write(T& out) {
    out.write(data);
    out.write(status);
  }
}; // PayloadData
 
 
class PayloadPackets {
public:
  std::vector<FelixPacket> packets;
  std::vector<uint32_t> status;
 
  uint64_t start_time = 0; // timestamp from ROB
  uint32_t source_id = 0;  // source id from ROB
 
  inline uint32_t get_data_size() {
    uint32_t size = 0;
    for (auto& packet : packets) size += get_packet_size(packet);
    return size;
  }
 
  inline uint32_t get_status_size() { return status.size(); }
 
  template<typename T>
  inline void write(T& out) {
    log.debug("PacketPayload.data");
    write_packets(out);
    log.debug("PacketPayload.status");
    out.write(status);
  }
 
  template<typename T>
  void write(T& out, FelixPacket& packet) {
 
    FLX::Header header;
 
    header.format_type = 0x0;
 
    header.felix_status = packet.status;
 
    header.elink_id = (packet.fid >> 16); // Vlink[1].ElinkID[19]
    // log.debug("packet.fid: 0x{:016x} | link_id: 0x{:x}", packet.fid, uint32_t(header.elink_id));
    auto felix_source = packet.fid >> 36;
    if (felix_source != source_id) {
      // flag wrong felix device
    }
 
    header.timestamp = packet.time - start_time;
 
    auto data32 = reinterpret_cast<const uint32_t*>(packet.data);
    auto packet_size = packet.size;
 
    while (packet_size) {
      uint32_t size32;
      if (packet_size < FLX::MAX_DATA32_SIZE*4) {
        size32 = (packet_size + 3)/4;
        header.padding_bytes = size32*4 - packet_size;
        packet_size = 0;
        // log.debug("padding_bytes: {}", uint8_t(header.padding_bytes));
      } else {
        size32 = FLX::MAX_DATA32_SIZE;
        header.padding_bytes = 0;
        packet_size -= FLX::MAX_DATA32_SIZE*4;
      }
 
      header.fragment_size = size32 + FLX::HEADER_SIZE;
      out.write_rec(header);
      out.write(data32, size32);
      data32 += size32;
    }
  }
 
  inline uint32_t get_packet_size(FelixPacket& packet) {
    return sizeof(FLX::Header)/4 + (packet.size + 3)/4;
  }
 
  template<typename T>
  void write_packets(T& out) {
    for (auto& packet : packets) write(out, packet);
  }
 
}; // PayloadPackets
 
 
template<typename Payload>
class Fragment {
public:
  Header header;
  Payload payload;
  Trailer trailer;
 
  inline uint32_t get_size() {
    return ROD::HEADER_SIZE + ROD::TRAILER_SIZE +
      payload.get_data_size() + payload.get_status_size();
  }
 
  template<typename T>
  void write(T& out) {
 
    log.debug("ROD.Header");
    out.write_rec(header);
 
    log.debug("ROD.Payload");
    payload.write(out);
 
    trailer.data_size   = payload.get_data_size();
    trailer.status_size = payload.get_status_size();
    trailer.status_pos  = 1;  // 1 - data before status
 
    log.debug("ROD.Trailer");
    out.write_rec(trailer);
  }
}; // Fragment
  
} // ROD
 
 
namespace ROB { // Read-Out Buffer
 
template<int n, typename RODFragment>
class Fragment {
public:
 
  ROB::Header<n> header;
  RODFragment rod;
 
  uint32_t get_size() {
    uint32_t frag_size = header.HEADER_SIZE + rod.get_size();
    if (header.checksum_type) frag_size += 1;
    return frag_size;
  }
 
  template<typename T>
  void write(T& out) {
    auto start_pos = out.get_size();
    // header.checksum_type = ROB::CHECKSUM_NONE;
    header.checksum_type = CHECKSUM_CRC16;
    uint32_t subdet_id = 0x16;
    uint32_t module_id = 0x0001;
    header.source_id = subdet_id << 16 | module_id;
    header.fragment_size = get_size();
 
    // writing ROB fragment
 
    log.debug("ROB.Header");
    out.write_rec(header); // writing ROB header
 
    rod.write(out); // writing ROD fragment
 
    if (header.checksum_type) {
      uint32_t checksum = calc_checksum(header.checksum_type);
      log.debug("ROB.CheckSum: 0x{:08x}", checksum);
      out.write(checksum); // writing checksum
    }
    auto end_pos = out.get_size();
    assert(end_pos - start_pos == header.fragment_size);
    if (end_pos - start_pos != header.fragment_size) {
      log.error("start: {}, end: {}, frag_size: {}", start_pos, end_pos, int(header.fragment_size));
    }
  }
 
  uint32_t checksum_CRC16() {
    return 0x11223344;
  }
 
  uint32_t checksum_Adler32() {
    return 0x55667788;
  }
 
  uint32_t calc_checksum(uint32_t type) {
    if (type == CHECKSUM_CRC16)   return checksum_CRC16();
    if (type == CHECKSUM_ADLER32) return checksum_Adler32();
    return 0xFFFFFFFF;
  }
 
}; // Fragment
 
} // ROB
 
 
 
class FragWriter {
public:
  FragWriter() = default;
 
  inline size_t get_size() {
    return frag.size();
  }
 
  inline void write(uint32_t data) {
    log.trace("{:3}: write: 0x{:08x}", frag.size(), data);
    frag.push_back(data);
  }
 
  inline void write(const uint32_t* data, size_t size) {
    for (uint32_t i = 0; i < size; i++) write(data[i]);
  }
 
  inline void write(const void* ptr, size_t size) {
    auto buff = reinterpret_cast<const uint32_t*>(ptr);
    write(buff, size);
  }
 
  inline void write(const std::span<const uint32_t> buff) {
    write(buff.data(), buff.size());
  }
 
  template<class T>
  inline void write_rec(const T& rec) {
    assert(sizeof(rec) % 4 == 0);
    auto ptr = reinterpret_cast<const uint32_t*>(&rec);
    write(ptr, sizeof(rec)/4);
  }
 
  std::vector<uint32_t> frag;
 
}; // FragWriter
 
 
 
 
template<typename DataCallback>
class FragReader {
public:
  DataCallback& cb_data;
 
  FragReader(DataCallback& cb_data) : cb_data(cb_data) {}
 
  inline uint32_t read() {
    // log.trace("read: 0x{:08x}", data);
    // frag.push_back(data);
    return 0;
  }
 
  template<int n>
  inline std::array<uint32_t, n> read() {
    return {};
  }
 
  template<typename T>
  void read_rob(T& buff) {
    // log.trace("read ROB | frag_size: {}", buff.size());
    if (buff.size() < ROB::BASE_HEADER_SIZE) {
      // Too short ROB fragment
    }
    if (buff[0] != ROB::HEADER_MARKER) {
      // incorrect ROB marker
    }
 
    assert(buff.size() > ROB::BASE_HEADER_SIZE);
    assert(buff[0] == ROB::HEADER_MARKER);
 
    // ROB::BaseHeader& header = *reinterpret_cast<ROB::BaseHeader*>(buff.data());
    ROB::BaseHeader& header = *reinterpret_cast<ROB::BaseHeader*>(buff.data());
 
    // Header sizes
    auto generic_size = ROD::HEADER_SIZE + header.status_size + 1;
 
    // Fragment consistency should be properly handled beforehand.
    // These assertions are added just to ensure that fragment is consistent.
 
    assert(header.fragment_size <= buff.size() && "Truncated fragment");
    assert(header.header_size <= header.fragment_size && "Inconsistent fragment and header sizes");
    assert(generic_size <= header.header_size && "Incorrect header sizes");
 
    auto checksum_type = header.status[header.status_size];
    auto checksum_size = (checksum_type ? 1 : 0);
 
    auto specific_size = header.header_size - generic_size;
 
    std::span<const uint32_t> specific {
      buff.data() + generic_size,
      header.header_size - generic_size
    };
 
    std::span<const uint32_t> payload {
      buff.data() + header.header_size,
      header.fragment_size - header.header_size - checksum_size
    };
 
    read_rod(payload);
  }
 
  template<typename T>
  void read_rod(T& buff) {
    // log.trace("read ROD | frag_size: {}", buff.size());
    if (buff.size() < ROD::HEADER_SIZE) {
      // Too short ROD fragment
    }
    if (buff[0] != ROD::HEADER_MARKER) {
      // Incorrect ROD marker
    }
 
    assert(buff.size() >= ROD::HEADER_SIZE);
    assert(buff[0] == ROD::HEADER_MARKER);
 
    auto& header  = *reinterpret_cast<const ROD::Header*>(buff.data());
 
    auto& trailer = *reinterpret_cast<const ROD::Trailer*>(
      (buff.data() + buff.size() - ROD::TRAILER_SIZE));
 
    std::span<const uint32_t> data {
      buff.data() + ROD::HEADER_SIZE + (trailer.status_pos & 0x1 ? 0 : trailer.status_size),
      trailer.data_size
    };
 
    std::span<const uint32_t> status {
      buff.data() + ROD::HEADER_SIZE + (trailer.status_pos & 0x1 ? trailer.data_size : 0),
      trailer.status_size
    };
 
    read_packets(data);
  }
 
  template<typename T>
  void read_packets(T& buff) {
 
    // log.trace("read packets | size: {}", buff.size());
    // for (auto data : buff) log.trace("  0x{:08x}", data);
 
    auto buff_data = buff.data();
    int buff_size = buff.size();
 
    while (buff_size > 0) {
      assert(buff_size >= FLX::HEADER_SIZE);
 
      uint64_t rob_fid = 0x1000000000000000L;
 
      const FLX::Header& header = *reinterpret_cast<const FLX::Header*>(buff_data);
 
      if (header.fragment_size < FLX::HEADER_SIZE) {
        log.error("too small fragment size: {}", header.fragment_size);
        break;
      }
 
      // log.info("buff_size: {}, fragment_size: {}", buff_size, int(header.fragment_size));
 
      auto  data = reinterpret_cast<const uint8_t*>(buff_data + FLX::HEADER_SIZE);
      uint32_t size = (header.fragment_size - FLX::HEADER_SIZE)*4 - header.padding_bytes;
 
      uint64_t fid = rob_fid | (uint64_t(header.elink_id) << 16);
      uint8_t  status = header.felix_status;
 
      cb_data.on_data(fid, data, size, status);
 
      buff_data += header.fragment_size;
      buff_size -= header.fragment_size;
    }
  }
 
  std::vector<uint32_t> frag;
 
}; // FragReader
 
 
} // itk::fragment