vmm_config.cpp

#include "vmm_config.h"
#include <sstream>
#include <bitset>
#include <algorithm>
#include <chrono>
#include <thread>
#include <array>

#include "FrontEndBase.h"

void delayMilliseconds(int milliseconds) {
  std::this_thread::sleep_for(std::chrono::milliseconds(milliseconds));
}

FENConfigModule::FENConfigModule(FrontEndBase& frontend) : pFEN(&frontend) {
}

FENConfigModule::~FENConfigModule() {
}

void FENConfigModule::acquire(bool acquire) {
  std::string message;
  std::vector<std::string> linkStatus;
  if (acquire) {
    enableAcquisition(false);
    sendAll(true);
    enableAcquisition(true);
    checkLinkStatus(message, linkStatus);
  } else {
    enableAcquisition(false);
    checkLinkStatus(message, linkStatus);
  }
}

void FENConfigModule::sendAll(bool useConfigCheck) {
  configFEN();
  for(int hybrid = 0; hybrid <= HYBRIDS_PER_FEN; hybrid++) {
    if (isHybridEnabled(hybrid)) {
      configHybrid(hybrid);

      for(int vmm = 0; vmm < VMMS_PER_HYBRID; vmm++) {
        configVMM(hybrid, vmm, useConfigCheck);
      }
    }
  }
}

void FENConfigModule::enableAcquisition(bool enabled) {
  pFEN->userRegWrite("app_acq_enable", enabled);
  pFEN->userRegWrite("sc_acq_on_off", 0x00000000);
  pFEN->userRegWrite("sc_acq_on_off", 0x00000001);
  pFEN->userRegWrite("sc_acq_on_off", 0x00000000);
}

void FENConfigModule::checkLinkStatus(std::string& message, std::vector<std::string>& linkStatus) {
  linkStatus.clear();
  for (int n = 0; n < HYBRIDS_PER_FEN; n++) linkStatus.push_back("0");

  uint32_t result = readWriteRegs("sc_app_link_status", 0, "app_link_status");

  std::stringstream ss;

  for (int n = 0; n < HYBRIDS_PER_FEN; n++) {
    if (isHybridEnabled(n)) {
      linkStatus[n] = std::to_string(result);
      ss << "hybr. " + std::to_string(n) + ": " + linkStatus[n] << std::endl;
    }
  }
  message = ss.str();
  std::cout << "Message: " << message << std::endl;
}

void FENConfigModule::configFEN() {
  pFEN->userRegWrite("app_debug_data_format", 0);
  pFEN->userRegWrite("app_latency_reset", 27);
  pFEN->userRegWrite("app_latency_data_max", 1024);
  pFEN->userRegWrite("app_latency_data_jitter", 0);
  pFEN->userRegWrite("app_tp_offset_first", 100);
  pFEN->userRegWrite("app_tp_offset", 1000);
  pFEN->userRegWrite("app_tp_latency", 46);
  pFEN->userRegWrite("app_tp_number", 1);
  pFEN->userRegWrite("app_chmask", getChMap());
  pFEN->userRegWrite("sc_cfg_app", 0x00000000);
  pFEN->userRegWrite("sc_cfg_app", 0x00000001);
  pFEN->userRegWrite("sc_cfg_app", 0x00000000);
}

uint16_t FENConfigModule::getChMap() {
  std::string chMapString = "0000000000000000";
  for (int hybrid = 0; hybrid < HYBRIDS_PER_FEN; hybrid++) {
    if (isHybridEnabled(hybrid)) {
      for (int vmm = 0; vmm < VMMS_PER_HYBRID; vmm++) {
        chMapString.replace(15 - (hybrid * 2 + vmm), 1, "1");
      }
    }
  }
  uint16_t chMap = std::stoi(chMapString, nullptr, 2);
  return chMap;
}

bool FENConfigModule::enableHybrid(int hybrid, bool onOff) {
  if(hybrid < HYBRIDS_PER_FEN) {
    enabled_hybrids[hybrid] = onOff;
    return true;
  } else return false;
}

bool FENConfigModule::isHybridEnabled(int hybrid) {
  if(hybrid < HYBRIDS_PER_FEN) return enabled_hybrids[hybrid];
  else return false;
}

Hybrid& FENConfigModule::getHybrid(int index) {
  if (index < 0 || index >= HYBRIDS_PER_FEN)
    throw std::out_of_range("Invalid hybrid index");
  return hybrids[index];
}

void FENConfigModule::configHybrid(int hybrid_index) {
  std::array<std::string, 3> registers = {"hyb_tp_skew0", "hyb_tp_width0", "hyb_tp_polarity0"};

  for (const auto& reg : registers) {
    unsigned short result = 0;
    hybrids[hybrid_index].getRegister(reg, result);
    pFEN->userRegWrite(reg + std::to_string(hybrid_index), result);
  }

  uint32_t value = (1 << hybrid_index);
  pFEN->userRegWrite("sc_cfg_hyb", 0x0);
  pFEN->userRegWrite("sc_cfg_hyb", value);
  pFEN->userRegWrite("sc_cfg_hyb", 0x0);
}

bool FENConfigModule::configVMM(int hybrid_index, int vmm_index, bool enableConfigCheck)
{
  bool result = true;
  bool ok;

  std::vector<std::string> globalRegisters;
  globalRegisters.clear();
  fillGlobalRegisters(globalRegisters, hybrid_index,  vmm_index);
  
  std::cout << "Global Registers:" << std::endl;
    for (const auto& reg : globalRegisters) {
      std::cout << reg << std::endl;
    }
  
  if(globalRegisters.size()!=3){
    std::cout << "ERROR Global SPI does not have 3 words" << std::endl;
    return -1;
  }

  std::vector<std::string> channelRegisters;
  channelRegisters.clear();
  fillChRegisters(channelRegisters, hybrid_index,  vmm_index);
  if(channelRegisters.size() != 64){
    std::cout << "ERROR Channel registers do not have 64 values" << std::endl;
    return -1;
  }

  std::vector<std::string> globalRegisters2;
  globalRegisters2.clear();
  fillGlobalRegisters2(globalRegisters2, hybrid_index,  vmm_index);
  if(globalRegisters2.size()!=3){
      std::cout << "ERROR Global SPI does not have 3 words" << std::endl;
      return -1;
  }

  int idx = hybrid_index * 2 + vmm_index;
  std::string sVmmIndex = "99";
  if(idx < 10) sVmmIndex = "0" + std::to_string(idx);
  else if(idx < 16) sVmmIndex = std::to_string(idx);
  
  bool g_use_config_check = true;
  if(g_use_config_check) {
    //reset I2C address 65 register 0
    if(enableConfigCheck) {
      // TODO: Check with we need that (the enableConfigCheck is always false in the VMM code) 
      //uint32_t value = (1 << hybrid_index);
      //ESS_WriteSc("sc_i2c_reset_config_check", 0x00000000, ok);
      //ESS_WriteSc("sc_i2c_reset_config_check", value, ok);
      //ESS_WriteSc("sc_i2c_reset_config_check", 0x00000000, ok);
    }
  }

  unsigned int firstGlobalRegSPI_2 = 0;
  unsigned int lastGlobalRegSPI_2 = 2;
  unsigned int firstGlobalRegSPI_1 = 67;
  unsigned int lastGlobalRegSPI_1 = 69;

  // global SPI / VMM3: global bank 0
  for(unsigned int i = firstGlobalRegSPI_2; i <= lastGlobalRegSPI_2; i++) {
    std::string param = "vmm_global_bank2_sp" + std::to_string(i - firstGlobalRegSPI_2) + sVmmIndex;
    uint32_t value = std::stoul(globalRegisters2[i - firstGlobalRegSPI_2], nullptr, 2);
    pFEN->userRegWrite(param, value);
  }

  //channel SPI
  for(unsigned int i = 0; i < CHANNELS_PER_VMM; i++) {
    std::string position_str = std::to_string(i);
    if (position_str.size() == 1) position_str = "0" + position_str;
    std::string param = "vmm_ch" + position_str + sVmmIndex;
    uint32_t value = std::stoul(channelRegisters[i], nullptr, 2);
    std::cout << "param: " << param << std::endl;
    pFEN->userRegWrite(param, value);
  }

  // global SPI / VMM3: global bank 1
  for(unsigned int i = firstGlobalRegSPI_1; i <= lastGlobalRegSPI_1; i++) {
    std::string param = "vmm_global_bank1_sp" + std::to_string(i - firstGlobalRegSPI_1) + sVmmIndex;
    uint32_t value = std::stoul(globalRegisters[i - firstGlobalRegSPI_1], nullptr, 2);
    pFEN->userRegWrite(param, value);
  }

  uint32_t value = (1 << idx);
  pFEN->userRegWrite("sc_cfg_vmm", 0x00000000);
  pFEN->userRegWrite("sc_cfg_vmm", value);
  pFEN->userRegWrite("sc_cfg_vmm", 0x00000000);
  
  return result;
}

void FENConfigModule::fillGlobalRegisters(std::vector<std::string>& global, int hybrid_index, int vmm_index)
{
  unsigned short result;

  global.clear();
  int sequence = 0;
  
  // GLOBAL SPI 0
  std::string spi0 = "00000000000000000000000000000000";

  //[0,3] reserved
  sequence += 4;

  // Function to replace a bit in the spi0 string with a value from VMM settings
  auto replaceVMMBit = [&](int index, const std::string& settingName) {
    // Convert value to '1' if it's not zero, otherwise '0'
    hybrids[hybrid_index].getVMM(vmm_index).getRegister(settingName, result);
    spi0[index] = '0' + (result != 0); 
  };

  const std::vector<std::string> settings = {"slvs", "s32", "stcr", "ssart", "srec", "stlc", "sbip", "srat", "sfrst",
                                            "slvsbc", "slvstp", "slvstk", "slvsdt", "slvsart", "slvstki", "slvsena",
                                            "slvs6b", "slh", "slxh", "stgc", "reset1", "reset2"};

  for (const auto& setting : settings) {
    if (setting == "reset1") sequence += 5; //[25,29] not used
    replaceVMMBit(sequence, setting);
    sequence++;
  }

  global.push_back(spi0);

  // GLOBAL SPI 1
  // Define a function to populate spi string based on VMM settings
  auto populateSPI1 = [&](const std::vector<std::pair<std::string, int>>& settings) {
    std::string spi;
    int sequence = 0;
    for (const auto& setting : settings) {
      hybrids[hybrid_index].getVMM(vmm_index).getRegister(setting.first, result);
      std::string binary_string = std::bitset<16>(result).to_string().substr(16 - setting.second);

      if (setting.first == "sdt") {
        // Populate threshold DAC lowest 6 bits
        for (int i = 4; i < 10; ++i) spi += binary_string[i];
      } else spi += binary_string;
      sequence += setting.second;
    }

    // Append '0' to the end of spi to reserve the last bit: [31] reserved
    spi += '0';

    // Push back the populated spi string to the global vector
    global.push_back(spi);
  };

  // Populate SPI 1 settings
  populateSPI1({{"sdt", 10},{"sdp10", 10}, {"sc10b", 2}, {"sc8b", 2},
                {"sc6b", 3}, {"s8b", 1}, {"s6b", 1}, {"s10b", 1},
                {"sdcks", 1}, {"sdcka", 1}, {"sdck6b", 1}, {"sdrv", 1},
                {"stpp", 1}});

  // GLOBAL SPI 2
  // Define a function to populate spi string based on VMM settings
  auto populateSPI2 = [&](const std::vector<std::pair<std::string, int>>& settings) {
    std::string spi;
    int sequence = 0;

    for (const auto& setting : settings) {
      hybrids[hybrid_index].getVMM(vmm_index).getRegister(setting.first, result);
      std::string binary_string = std::bitset<16>(result).to_string().substr(16 - setting.second);
      if (setting.first == "sdt") {
        // Populate threshold DAC highest 4 bits
        for (int i = 0; i < 4; ++i) {
          spi += binary_string[i];
          sequence++;
        }
      } else {
        spi += binary_string;
        sequence += setting.second;
      }
    }
    // Push back the populated spi string to the global vector
    global.push_back(spi);
  };

  // Populate SPI 2 settings
  populateSPI2({{"sp", 1}, {"sdp", 1}, {"sbmx", 1}, {"sbft", 1}, {"sbfp", 1},
                {"sbfm", 1}, {"slg", 1}, {"sm5_sm0", 6}, {"scmx", 1}, {"sfa", 1},
                {"sfam", 1}, {"st", 2}, {"sfm", 1}, {"sg", 3}, {"sng", 1}, {"stot", 1},
                {"sttt", 1}, {"ssh", 1}, {"stc", 2}, {"sdt", 10}});

}

void FENConfigModule::fillChRegisters(std::vector<std::string>& registers, int hybrid_index, int vmm_index)
{
  registers.clear();
  unsigned short result;

  auto populateRegisters = [&](const std::vector<std::pair<std::string, int>>& regs, int channel) {
    std::string _reg = "00000000";
    int sequence = 8;

    for (const auto& reg : regs) {
      hybrids[hybrid_index].getVMM(vmm_index).getRegister(reg.first, channel, result);
      std::string binary_string = std::bitset<16>(result).to_string().substr(16 - reg.second);
      if (reg.first == "sd" || reg.first == "sz10b" ||
          reg.first == "sz08b" || reg.first == "sz06b") {
        //According to ATLAS software, has to be reversed
        std::reverse(binary_string.begin(), binary_string.end());
      }
      _reg += binary_string;
      sequence += reg.second;
    }

    // Append '0' to the end of spi to reserve the last bit: [31] reserved
    _reg += "0";

    // Push back the populated _reg string to the registers vector
    registers.push_back(_reg);
  };

  std::vector<std::pair<std::string, int>> registers_map = {{"sc", 1}, {"sl", 1}, {"st", 1},
                                                        {"sth", 1}, {"sm", 1}, {"smx", 1},
                                                        {"sd", 5}, {"sz10b", 5}, {"sz08b", 4},
                                                        {"sz06b", 3}};

  for(int ch = 0; ch < CHANNELS_PER_VMM; ch++) {
    populateRegisters(registers_map, ch);
  }
}

void FENConfigModule::fillGlobalRegisters2(std::vector<std::string>& global, int hybrid_index, int vmm_index)
{
  unsigned short result;
  int sequence = 0;

  // GLOBAL SPI 0
  std::string spi0 = "00000000000000000000000000000000";
  //[0,30] not used
  sequence+=31;

  // magic number on BCID [31]
  hybrids[hybrid_index].getVMM(vmm_index).getRegister("nskipm_i", result);
  spi0.replace(sequence, 1, std::to_string(result));
  
  global.push_back(spi0);

  // GLOBAL SPI 1
  std::string spi1 = "00000000000000000000000000000000";
  hybrids[hybrid_index].getVMM(vmm_index).getRegister("sL0cktest", result);
  // clocks when L0 core disabled [0]
  spi1.replace(sequence, 1, std::to_string(result));
  sequence++;

  //invert DCK [1]
  hybrids[hybrid_index].getVMM(vmm_index).getRegister("sL0dckinv", result);
  spi1.replace(sequence, 1, std::to_string(result));
  sequence++;

  //invert BCCLK [2]
  hybrids[hybrid_index].getVMM(vmm_index).getRegister("sL0ckinv", result);
  spi1.replace(sequence, 1, std::to_string(result));
  sequence++;

  //L0 core
  hybrids[hybrid_index].getVMM(vmm_index).getRegister("sL0ena", result);
  spi1.replace(sequence, 1, std::to_string(result));
  sequence++;

  global.push_back(spi1);

  // GLOBAL SPI 2
  std::string spi2 = "00000000000000000000000000000000";
  global.push_back(spi2);
}

int FENConfigModule::readADC(int hybrid_index, int vmm_index) {
  int adc_result = 0;
  int vmm = hybrid_index * 2 + vmm_index;
  std::string sVmmIndex = "99";
  if(vmm < 10) sVmmIndex = "0" + std::to_string(vmm);
  else if(vmm < 16) sVmmIndex = std::to_string(vmm);

  uint32_t result = readWriteRegs("sc_i2c_vmm_adc", vmm, "vmm_i2c_adc" + sVmmIndex);
  adc_result = (result & 0xFFFF) >> 4; // bit shift

  unsigned short sp_value = 0;
  hybrids[hybrid_index].getVMM(vmm_index).getRegister("sp", sp_value);
  unsigned sm5_sm0_value = 0;
  hybrids[hybrid_index].getVMM(vmm_index).getRegister("sm5_sm0", sp_value);

  if((sp_value != 0) && (sm5_sm0_value == 1)) adc_result = 1200 - adc_result;

  return adc_result;
}

uint32_t FENConfigModule::readWriteRegs(std::string command_reg, int index, std::string read_reg) {
  unsigned int data = 0x00000000;
  pFEN->userRegWrite(command_reg, data);
  data = (1 << index);
  pFEN->userRegWrite(command_reg, data);

  //Do not read registers back too soon, otherwise old ADC value is read
  delayMilliseconds(7);

  uint32_t value = pFEN->userRegRead(read_reg);
  std::cout << "value: " << value << std::endl;
  pFEN->userRegWrite(command_reg, 0x0);

  return value;
}