/**
 * Copyright (C) 2016-2019 Xilinx, Inc
 *
 * Licensed under the Apache License, Version 2.0 (the "License"). You may
 * not use this file except in compliance with the License. A copy of the
 * License is located at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations
 * under the License.
 */

/**
 * Embedded runtime scheduler
 */

#include "core/include/ert.h"
// includes from bsp
#ifndef ERT_HW_EMU
#include <xil_printf.h>
#include <mb_interface.h>
#include <xparameters.h>
#else
#include <stdio.h>
#define xil_printf printf
#define print printf
#define u32 uint32_t
#endif
#include <stdlib.h>
#include <limits>
#include <bitset>

// version is a git hash passed in from build script
// default for builds that bypass build script
#ifndef ERT_VERSION
# define ERT_VERSION 0
#endif
#ifndef ERT_SVERSION
# define ERT_SVERSION "0xdeadbeef"
#endif

// set local string that can be extracted
// from binary using 'strings sched.bin'
const char* ert_version = ERT_SVERSION;

#define ERT_UNUSED __attribute__((unused))

//#define ERT_VERBOSE
//#define CTRL_VERBOSE
//#define DEBUG_SLOT_STATE

// Assert macro implementation
ERT_UNUSED
static void
ert_assert(const char* file, long line, const char* function, const char* expr, const char* msg)
{
  xil_printf("Assert failed: %s:%ld:%s:%s %s\n",file,line,function,expr,msg);
  exit(1);
}

#ifdef ERT_VERBOSE
# define ERT_PRINT(msg) print(msg)
# define ERT_DEBUG(msg) print(msg)
# define ERT_PRINTF(format,...) xil_printf(format, ##__VA_ARGS__)
# define ERT_DEBUGF(format,...) xil_printf(format, ##__VA_ARGS__)
# define ERT_ASSERT(expr,msg) ((expr) ? ((void)0) : ert_assert(__FILE__,__LINE__,__FUNCTION__,#expr,msg))
#else
# define ERT_PRINT(msg) print(msg)
# define ERT_DEBUG(msg)
# define ERT_PRINTF(format,...) xil_printf(format, ##__VA_ARGS__)
# define ERT_DEBUGF(format,...)
# define ERT_ASSERT(expr,msg)
//# define ERT_ASSERT(expr,msg) ((expr) ? ((void)0) : ert_assert(__FILE__,__LINE__,__FUNCTION__,#expr,msg))
#endif

#ifdef CTRL_VERBOSE
# define CTRL_DEBUG(msg) print(msg)
# define CTRL_DEBUGF(format,...) xil_printf(format, ##__VA_ARGS__)
#else
# define CTRL_DEBUG(msg)
# define CTRL_DEBUGF(format,...)
#endif

#ifdef ERT_HW_EMU
using addr_type = uint32_t;
using value_type = uint32_t;

extern value_type read_reg(addr_type addr);
extern void write_reg(addr_type addr, value_type val);
extern void microblaze_enable_interrupts();
extern void microblaze_disable_interrupts();
extern void reg_access_wait();
#endif

namespace ert {
////////////////////////////////////////////////////////////////
// Convenience types for clarity
////////////////////////////////////////////////////////////////
using size_type = uint32_t;
using addr_type = uint32_t;
using value_type = uint32_t;
using bitmask_type = uint32_t;

////////////////////////////////////////////////////////////////
// HLS AXI-lite
////////////////////////////////////////////////////////////////
static const u32 AP_START    = 0x1;
static const u32 AP_DONE     = 0x2;
static const u32 AP_IDLE     = 0x4;
static const u32 AP_READY    = 0x8;
static const u32 AP_CONTINUE = 0x10;

////////////////////////////////////////////////////////////////
// HLS AXI protocol (from xclbin.h)
////////////////////////////////////////////////////////////////
static const u32 AP_CTRL_HS    = 0;
static const u32 AP_CTRL_CHAIN = 1;
static const u32 AP_CTRL_NONE  = 2;
static const u32 AP_CTRL_ME    = 3;

////////////////////////////////////////////////////////////////
// Extensions to core/include/ert.h
////////////////////////////////////////////////////////////////
addr_type STATUS_REGISTER_ADDR[4] = {0, 0, 0, 0};

addr_type CU_DMA_REGISTER_ADDR[4] = {0, 0, 0, 0};

addr_type CU_STATUS_REGISTER_ADDR[4] = {0, 0, 0, 0};

addr_type CQ_STATUS_REGISTER_ADDR[4] = {0, 0, 0, 0};

/**
 * Simple bitset type supporting 128 bits
 *
 * ERT supports a max of 128 CUs and 128 slots, this bitset class
 * is added to simplify managing 4 32bit bitmasks.
 *
 * Using std::bitset non-throwing functions only
 */
using bitset_type = std::bitset<128>;

// If this assert fails, then ert_parameters is out of sync with
// the board support package header files.
#ifndef ERT_HW_EMU
static_assert(ERT_INTC_ADDR==XPAR_INTC_SINGLE_BASEADDR,"update core/include/ert.h");
#endif

// Marker for invalid index
const size_type no_index = std::numeric_limits<size_type>::max();

////////////////////////////////////////////////////////////////
// Configuarable constants
// Statically allcoated array size is reduced in debug otherwise
// there is not enough space for compiled firmware
////////////////////////////////////////////////////////////////
#ifdef ERT_VERBOSE
const  size_type max_slots                  = 32;   // size of statically allocated array
#else
const  size_type max_slots                  = 128;  // size of statically allocated array
#endif
static size_type num_slots                  = 16;   // actual number of slots
static size_type num_slot_masks             = 1;    // (num_slots-1>>5)+1;

// Max number of compute units
#ifdef ERT_VERBOSE
const  size_type max_cus                    = 32;   // size of statically allocated array
#else
const  size_type max_cus                    = 128;  // size of statically allocated array
#endif
static size_type num_cus                    = 3;    // actual number of cus
static size_type num_cu_masks               = 1;    // (num_cus-1>>5)+1;

// CU base address
static addr_type cu_base_address            = 0x0;

// CU address map
static addr_type cu_addr_map[max_cus]       = {0};

// CU offset (addone is 64k (1<<16), OCL is 4k (1<<12))
static size_type cu_offset                  = 16;

// Slot size
static size_type slot_size                  = 0x1000;

// Enable features via  configure_mb
static value_type cu_dma_enabled            = 0;
static value_type cu_interrupt_enabled      = 0;
static value_type cq_status_enabled         = 0;
static value_type mb_host_interrupt_enabled = 0;
static value_type cu_dma_52                 = 0;
static value_type cdma_enabled              = 0;
static value_type dataflow_enabled          = 0;

// Struct slot_info is per command slot in command queue
struct slot_info
{
  // Address of slot in command queue
  addr_type slot_addr = 0;

  // Last command header read from slot in command queue
  // Last 4 bits of header are used for slot status per mb state
  // new     [0x1]: the command is in new state per host
  // queued  [0x2]: the command is queued in MB.
  // running [0x3]: the command is running
  // free    [0x4]: the command slot is free
  value_type header_value = 0;

  // Cache opcode
  value_type opcode = 0;

  // Index of CU that is assigned to this command
  size_type cu_idx = no_index;

  // Address of register map in command slot
  addr_type regmap_addr = 0;

  // Size of register map in command slot (in 32 bit words)
  size_type regmap_size = 0;
};

// Fixed sized map from slot_idx -> slot info
static slot_info command_slots[max_slots];

// Fixed sized map from cu_idx -> slot_idx
static size_type cu_slot_usage[max_cus];

// Fixed sized map from cu_idx -> number of times executed
static size_type cu_usage[max_cus];

// Bitmask indicating status of CUs. (0) idle, (1) running.
// Only 'num_cus' lower bits are used
static bitset_type cu_status;

// Bitmask for interrupt enabled CUs.  (0) no interrupt (1) enabled
static bitset_type cu_interrupt_mask;
#ifndef ERT_HW_EMU
/**
 * Utility to read a 32 bit value from any axi-lite peripheral
 */
inline value_type
read_reg(addr_type addr)
{
  volatile auto ptr = reinterpret_cast<addr_type*>(addr);
  return *ptr;
}

/**
 * Utility to write a 32 bit value from any axi-lite peripheral
 */
inline void
write_reg(addr_type addr, value_type val)
{
  volatile auto ptr = reinterpret_cast<addr_type*>(addr);
  *ptr = val;
}
#endif
/**
 * Command opcode [27:23]
 */
inline value_type
opcode(value_type header_value)
{
  return (header_value >> 23) & 0x1F;
}
  
/**
 * Command type [31:28]
 */
inline value_type
cmd_type(value_type header_value)
{
  return (header_value >> 28) & 0xF;
}

/**
 * Command header [22:12] is payload size
 */
inline size_type
payload_size(value_type header_value)
{
  return (header_value >> 12) & 0x7FF;
}

/**
 * Command header [11:10] is extra cu masks.
 */
inline size_type
cu_masks(value_type header_value)
{
  return 1 + ((header_value >> 10) & 0x3);
}

/**
 * CU section (where the cu bitmasks start)
 */
inline addr_type
cu_section_addr(addr_type slot_addr)
{
  return slot_addr + sizeof(addr_type);
}

/**
 * Regmap section (where the cu regmap) is immediately
 * after cu section
 */
inline addr_type
regmap_section_addr(value_type header_value,addr_type slot_addr)
{
  return cu_section_addr(slot_addr) + cu_masks(header_value)*sizeof(addr_type);
}

/**
 * Size of regmap is payload size (n) minus the number of cu_masks
 */
inline size_type
regmap_size(value_type header_value)
{
  return payload_size(header_value) - cu_masks(header_value);
}

inline addr_type
cu_idx_to_addr(size_type cu_idx)
{
  //  return (cu_idx << cu_offset) + cu_base_address;
  return cu_addr_map[cu_idx];
}

/**
 * idx_in_mask() - Check if idx in in specified 32 bit mask
 *
 * @idx: Index to check
 * @mask_idx: Index of bit mask determines range of mask (1=>[63,32])
 * Return: @true of idx is in range of 32 bit mask, @false otherwise
 */
inline bool
idx_in_mask(size_type idx, size_type mask_idx)
{
  return idx < ((mask_idx+1)<<5);
}

/**
 * idx_to_mask() - Return the bitmask corresponding to idx in mask with idx
 *
 * @idx: Index to map to specified mask
 * @mask_idx: Index of bit mask determines range of mask (1=>[63,32])
 * Return: 32 bit bitmask with position of translated idx set to '1', or 0x0
 *  if idx is not covered by range of mask
 */
inline bitmask_type
idx_to_mask(size_type idx, size_type mask_idx)
{
  return idx_in_mask(idx,mask_idx)
    ? 1 << (idx-(mask_idx<<5))
    : 0;
}

// scope guard for disabling interrupts
struct disable_interrupt_guard
{
  disable_interrupt_guard()
  {
    if (cu_interrupt_enabled || cq_status_enabled)
      write_reg(ERT_INTC_MER_ADDR,0x0); // interrupt controller master disable
  }
  ~disable_interrupt_guard()
  {
    if (cu_interrupt_enabled || cq_status_enabled)
      write_reg(ERT_INTC_MER_ADDR,0x3); // interrupt controller master enable
  }
};

inline static void
setup_ert_base_addr()
{
  /* In Subsytem 2.0, ERT MB now has to go around to access 3 peripherals
   * internal to ERT Subsystem i.e CQRAM Controller, Embedded scheduler HW
   * and KDMA. ERT MB needs to read the value in ERT_BASE_ADDR, add that
   * value to ERT Subsystem Base Address and Peripheral Address, new value
   * would be used by the ERT MB to access CQ and CSR.
   */
  #if defined(ERT_BUILD_V20)
  ert_base_addr = read_reg(ERT_BASE_ADDR);
  #endif
  STATUS_REGISTER_ADDR[0] = ERT_STATUS_REGISTER_ADDR0;
  STATUS_REGISTER_ADDR[1] = ERT_STATUS_REGISTER_ADDR1;
  STATUS_REGISTER_ADDR[2] = ERT_STATUS_REGISTER_ADDR2;
  STATUS_REGISTER_ADDR[3] = ERT_STATUS_REGISTER_ADDR3;
  CU_DMA_REGISTER_ADDR[0] = ERT_CU_DMA_REGISTER_ADDR0;
  CU_DMA_REGISTER_ADDR[1] = ERT_CU_DMA_REGISTER_ADDR1;
  CU_DMA_REGISTER_ADDR[2] = ERT_CU_DMA_REGISTER_ADDR2;
  CU_DMA_REGISTER_ADDR[3] = ERT_CU_DMA_REGISTER_ADDR3;
  CU_STATUS_REGISTER_ADDR[0] = ERT_CU_STATUS_REGISTER_ADDR0;
  CU_STATUS_REGISTER_ADDR[1] = ERT_CU_STATUS_REGISTER_ADDR1;
  CU_STATUS_REGISTER_ADDR[2] = ERT_CU_STATUS_REGISTER_ADDR2;
  CU_STATUS_REGISTER_ADDR[3] = ERT_CU_STATUS_REGISTER_ADDR3;
  CQ_STATUS_REGISTER_ADDR[0] = ERT_CQ_STATUS_REGISTER_ADDR0;
  CQ_STATUS_REGISTER_ADDR[1] = ERT_CQ_STATUS_REGISTER_ADDR1;
  CQ_STATUS_REGISTER_ADDR[2] = ERT_CQ_STATUS_REGISTER_ADDR2;
  CQ_STATUS_REGISTER_ADDR[3] = ERT_CQ_STATUS_REGISTER_ADDR3;
}

/**
 * MB configuration
 */
static void
setup()
{
  CTRL_DEBUG("-> setup\n");

  // In dataflow number of slots is number of CUs plus ctrl slot (0),
  // otherwise its as many slots as possible per slot_size
  num_slots = dataflow_enabled ? num_cus+1 : ERT_CQ_SIZE / slot_size;
  num_slot_masks = ((num_slots-1)>>5) + 1;
  num_cu_masks = ((num_cus-1)>>5) + 1;

  CTRL_DEBUGF("slot_size=0x%x\n",slot_size);
  CTRL_DEBUGF("num_slots=%d\n",num_slots);
  CTRL_DEBUGF("num_slot_masks=%d\n",num_slot_masks);
  CTRL_DEBUGF("num_cus=%d\n",num_cus);
  CTRL_DEBUGF("num_cu_masks=%d\n",num_cu_masks);
  CTRL_DEBUGF("cu_offset=%d\n",cu_offset);
  CTRL_DEBUGF("cu_base_address=0x%x\n",cu_base_address);
  CTRL_DEBUGF("cu_dma_enabled=%d\n",cu_dma_enabled);
  CTRL_DEBUGF("cu_dma_52=%d\n",cu_dma_52);
  CTRL_DEBUGF("cdma_enabled=%d\n",cdma_enabled);
  CTRL_DEBUGF("cu_isr_enabled=%d\n",cu_interrupt_enabled);
  CTRL_DEBUGF("cq_int_enabled=%d\n",cq_status_enabled);
  CTRL_DEBUGF("mb_host_int_enabled=%d\n",mb_host_interrupt_enabled);
  CTRL_DEBUGF("dataflow_enabled=%d\n",dataflow_enabled);

  // Initialize command slots
  for (size_type i=0; i<num_slots; ++i) {
    auto& slot = command_slots[i];
    slot.slot_addr = ERT_CQ_BASE_ADDR + (slot_size * i);
    slot.header_value = 0x4; // free
    slot.cu_idx = no_index;
    slot.regmap_addr = 0;
    slot.regmap_size = 0;

    // Clear command queue headers memory
    write_reg(slot.slot_addr,0x0);
  }

  // Clear CSR  (COR so read)
  for (size_type i=0; i<4; ++i)
    ERT_UNUSED volatile auto val = read_reg(STATUS_REGISTER_ADDR[i]);

  cu_status.reset();

  // Initialize cu_slot_usage
  for (size_type i=0; i<num_cus; ++i) {
    cu_slot_usage[i] = no_index;
    cu_usage[i] = 0;
  }

  // Set slot size (4K)
  write_reg(ERT_CQ_SLOT_SIZE_ADDR,slot_size/4);

  // CU offset in shift value
  if(!cu_dma_52)
    write_reg(ERT_CU_OFFSET_ADDR,cu_offset);

  // Number of command slots
  write_reg(ERT_CQ_NUMBER_OF_SLOTS_ADDR,num_slots);

  // CU base address
  if(!cu_dma_52)
    write_reg(ERT_CU_BASE_ADDRESS_ADDR,cu_base_address/4);

  // Command queue base address
#if defined(ERT_BUILD_U50) || defined(ERT_BUILD_V20)
  // The HW has changed so that the CQ is accessed at a different
  // address by the cudma_controller (which is internal to the ERT
  // subsystem) than the address it is accessed to my masters which
  // are external to the ERT subsystem e.g.XRT.
  //
  // So the cudma_controller has the CQ BRAM mapped at address
  // 0x0000_0000, but it will be at address ERT_CQ_BASE_ADDR
  // (i.e. 0x0034_0000) for XRT. And the code above was updated to
  // reflect this.
  write_reg(ERT_CQ_BASE_ADDRESS_ADDR,0x0);
#else
  write_reg(ERT_CQ_BASE_ADDRESS_ADDR,ERT_CQ_BASE_ADDR/4);
#endif

  // Number of CUs
  write_reg(ERT_NUMBER_OF_CU_ADDR,num_cus);

  // Enable/disable CU_DMA module
  write_reg(ERT_CU_DMA_ENABLE_ADDR,cu_dma_enabled);

  // Fill CU base addresses for cuisr
  if(cu_dma_52) {
    for (size_type i=0; i<num_cus; ++i) {
      write_reg(ERT_CUISR_LUT_ADDR+i*4,cu_addr_map[i]/4);
    }
  }

  bool enable_master_interrupts = false;

  // Enable cu interupts (cu -> cu_isr -> mb interrupts)
  cu_interrupt_mask.reset();
  bitmask_type intc_ier_mask = 0;
  if (cu_interrupt_enabled) {
    for (size_type cu=0; cu<num_cus; ++cu) {
      write_reg(cu_idx_to_addr(cu) + 0x04,1);
      write_reg(cu_idx_to_addr(cu) + 0x08,1);
      cu_interrupt_mask[cu] = 1;
    }
    write_reg(ERT_CU_ISR_HANDLER_ENABLE_ADDR,1); // enable CU ISR handler
    intc_ier_mask |= 0x2;                        // acccept cu interrupts on bit 1 of the ier of intc
    enable_master_interrupts = true;

    if (cdma_enabled)
      intc_ier_mask |= 0x4;                      // accept cdma interrupt on bit 2 of the ier of intc
  }
  else {
    for (size_type cu=0; cu<num_cus; ++cu) {
      write_reg(cu_idx_to_addr(cu) + 0x04,0);
      write_reg(cu_idx_to_addr(cu) + 0x08,0);
    }
    write_reg(ERT_INTC_IER_ADDR,read_reg(ERT_INTC_IER_ADDR) & ~0x6);  // disable interrupts on bit 1 & 2 (0x2|0x4)
    write_reg(ERT_CU_ISR_HANDLER_ENABLE_ADDR,0); // disable CU ISR handler
  }
  CTRL_DEBUGF("cu interrupt mask : %s\n", "cannot convert"); // to_string throws

  // Enable interrupts from host to MB when new commands are ready
  // When enabled, MB will read CQ_STATUS_REGISTER(s) to determine new
  // command slots.
  if (cq_status_enabled) {
    write_reg(ERT_CQ_STATUS_ENABLE_ADDR,1);   // enable feature
    intc_ier_mask |= 0x1;                     // acccept interrupts on bit 0 of the ier of intc
    enable_master_interrupts = true;
  }
  else {
    write_reg(ERT_INTC_IER_ADDR,read_reg(ERT_INTC_IER_ADDR) & ~0x1);  // disable interrupts on bit 0
    write_reg(ERT_CQ_STATUS_ENABLE_ADDR,0);      // disable feature
  }

  if (enable_master_interrupts) {
    write_reg(ERT_INTC_IER_ADDR,intc_ier_mask);  // accept interrupts for features
    write_reg(ERT_INTC_MER_ADDR,0x3);            // interrupt controller master enable
    microblaze_enable_interrupts();          // enable interrupts
  }
  else {
    write_reg(ERT_INTC_MER_ADDR,read_reg(ERT_INTC_MER_ADDR) & ~0x3);           // interrupt controller master enable
    microblaze_disable_interrupts();          // enable interrupts
  }

  // Enable/disable mb->host interrupts
  write_reg(ERT_HOST_INTERRUPT_ENABLE_ADDR,mb_host_interrupt_enabled);

  CTRL_DEBUG("<- setup\n");
}

/**
 * Associate CUs with a command slot
 */
inline void
set_cu_info(size_type cu_idx, size_type slot_idx)
{
  ERT_DEBUGF("cu_slot_usage[%d]=%d\n",cu_idx,slot_idx);
  ERT_ASSERT(cu_slot_usage[cu_idx]==no_index,"cu already used");
  cu_slot_usage[cu_idx] = slot_idx;
  ++cu_usage[cu_idx];
}

/**
 * write_command_status
 */
inline void
notify_host(size_type cmd_idx)
{
  // notify host (update host status register)
  ERT_DEBUGF("notify_host(%d)\n",cmd_idx);
#if 0
  auto mask_idx = cmd_idx>>5;
  auto mask = idx_to_mask(cmd_idx,mask_idx);
  write_reg(STATUS_REGISTER_ADDR[mask_idx],mask);
#else
  // this relies on 1<<n == 1<<(n%32), safe?  no, not safe, to be fixed
  // once 128 slots work so I can check the actual behavior
  write_reg(STATUS_REGISTER_ADDR[cmd_idx>>5],1<<cmd_idx);
#endif
}

/**
 * Configure a CU at argument address
 *
 * Write register map to CU control register at address
 *
 * @param cu_addr
 *  Address of CU control register
 * @param regmap_addr
 *  The address of the register map to copy into the CU control
 *  register
 * @param regmap_size
 *  The size of register map in 32 bit words
 */
inline void
configure_cu(addr_type cu_addr, addr_type regmap_addr, size_type regmap_size)
{
  // write register map, starting at base + 0x10
  // 0x4, 0x8, 0xc used for interrupt, which is initialized in setup
  for (size_type idx = 4; idx < regmap_size; ++idx)
    write_reg(cu_addr + (idx << 2), read_reg(regmap_addr + (idx << 2)));

  // start kernel at base + 0x0
  write_reg(cu_addr, 0x1);
}

/**
 * Configure CU with address value pairs (out-of-order)
 */
inline void
configure_cu_ooo(addr_type cu_addr, addr_type regmap_addr, size_type regmap_size)
{
  // write register map addr, value pairs starting 
  // past reserved 4 ctrl + 2 ctx 
  for (size_type idx = 6; idx < regmap_size; idx += 2) {
    addr_type offset = read_reg(regmap_addr + (idx << 2));
    value_type value = read_reg(regmap_addr + ((idx + 1) << 2));
    write_reg(cu_addr + offset, value);
  }

  // start kernel at base + 0x0
  write_reg(cu_addr, 0x1);
}

/**
 * Configure a CU DMA engine
 *
 * The engine will transfer CU register map and start the CU.
 *
 * @param cu_idx
 *  Index of CU to be configured by CUDMA
 * @param slot_idx
 *  Index of cmd slot from where CU regmap is to be copied by CUDMA
 * @param slot_addr
 *  The address of the slot corresponding to slot_idx (could be
 *  computed or looked up, but its already available by caller)
 */
inline void
configure_cu_dma(size_type cu_idx, size_type slot_idx, addr_type slot_addr)
{
  if (cu_dma_52) {
    // Write cu address to command queue slot.  This is used by DMA engine
    ERT_DEBUGF("writing cu_addr(0x%x) to slot cu_mask at address 0x%x\n",cu_idx_to_addr(cu_idx),cu_section_addr(slot_addr));
    write_reg(cu_section_addr(slot_addr),cu_idx_to_addr(cu_idx)>>2); //Shift by 2 since HLS kernel shifts up by 2
  }
  else {
    // Write cu_idx to command queue slot.  This is read by
    // DMA engine to find the CU to configure
    for (size_type mask_idx=0; mask_idx<num_cu_masks; ++mask_idx) {
      ERT_DEBUGF("writing cu_mask(0x%x) to slot cu_mask at address 0x%x\n"
                 ,idx_to_mask(cu_idx,mask_idx),cu_section_addr(slot_addr) + (mask_idx << 2));
      write_reg(cu_section_addr(slot_addr) + (mask_idx << 2), idx_to_mask(cu_idx,mask_idx));
    }
  }

  // Write the slot index to CU DMA register.  This is read by DMA
  // engine to find the CU register map to transfer to CU.
  size_type mask_idx = slot_idx>>5;
  ERT_DEBUGF("writing slot_mask 0x%x to cu_dma_queue at address 0x%x\n"
             ,idx_to_mask(slot_idx,mask_idx),CU_DMA_REGISTER_ADDR[mask_idx]);
  write_reg(CU_DMA_REGISTER_ADDR[mask_idx],idx_to_mask(slot_idx,mask_idx));
}

/**
 * Start a CU for command in slot
 *
 * @param slot_idx
 *  Index of command
 * @return
 *  Index of CU that was started or no_index if no CU was
 *  started (all were busy).
 *
 * Command is already assigned a CU by KDS.  This function checks
 * the current ERT status of that CU and starts it if it is unused.
 */
inline size_type
start_cu(size_type slot_idx)
{
  auto& slot = command_slots[slot_idx];
  auto cu_idx = slot.cu_idx;

  if (cu_status[cu_idx])
    return no_index;

  ERT_DEBUGF("start_cu cu(%d) for slot_idx(%d)\n",cu_idx,slot_idx);
  ERT_ASSERT(read_reg(cu_idx_to_addr(cu_idx))==AP_IDLE,"cu not ready");

  if (slot.opcode==ERT_EXEC_WRITE)
    // Out of order configuration
    configure_cu_ooo(cu_idx_to_addr(cu_idx),slot.regmap_addr,slot.regmap_size);
  else if (cu_dma_enabled && (cu_dma_52 || regmap_size(slot.header_value)<(127+4)))
    // Use CUDMA and adjust for 5.1 DSAs that have a bug and supports
    // at most 127 word copy excluding the 4 control words
    configure_cu_dma(cu_idx,slot_idx,slot.slot_addr);
  else
    // manually configure and start cu
    configure_cu(cu_idx_to_addr(cu_idx),slot.regmap_addr,slot.regmap_size);
  
  cu_status[cu_idx] = !cu_status[cu_idx];     // toggle cu status bit, it is now busy
  set_cu_info(cu_idx,slot_idx); // record which slot cu associated with
  return cu_idx;
}

/**
 * Check command status
 *
 * Called from CU interrupt service routine.  Argument CU is
 * complete, notify host and update state to indiciate slot
 * is now free.
 *
 * @param slot_idx
 *  Index of command slot that is associated with the CU
 * @param cu_idx
 *  Index of CU that just completed
 */
static inline void
check_command(size_type slot_idx, size_type cu_idx)
{
  auto& slot = command_slots[slot_idx];
  ERT_ASSERT(slot.cu_idx == cu_idx,"cu is not used by slot");
  notify_host(slot_idx);
  slot.header_value = (slot.header_value & ~0xF) | 0x4; // free
  ERT_DEBUGF("slot(%d) [running -> free]\n",slot_idx);

#ifdef DEBUG_SLOT_STATE
  write_reg(slot.slot_addr,slot.header_value);
#endif
}

/**
 * Check CU status and update internal state if complete
 &
 * @param cu_idx
 *   Index of CU to check
 * @return
 *   True if CU is done, false otherwise
 */
ERT_UNUSED
static inline bool
check_cu(size_type cu_idx)
{
  // cu interrupt enabled, then managed by interrupt handler
  if (cu_interrupt_mask[cu_idx])
    return false;

  // no interrupt for cu, check cu status register manually
  ERT_ASSERT(cu_status[cu_idx],"cu wasn't started");

  if (!(read_reg(cu_idx_to_addr(cu_idx)) & AP_DONE))
    return false;

  // toogle cu status bit, it is now free
  cu_status[cu_idx] = !cu_status[cu_idx];
  cu_slot_usage[cu_idx] = no_index; // reset slot index
  return true;
}

/**
 * Configure MB and peripherals
 *
 * Wait for CONFIGURE_MB in specified slot, then configure as
 * requested.
 *
 * This function is used in two different scenarios:
 *  1. MB reset/startup, in which case the CONFIGURE_MB is guaranteed
 *     to be in a slot at default slot offset (4K), most likely slot 0.
 *  2. During regular scheduler loop, in which case the CONFIGURE_MB
 *     packet is at an arbitrary slot location.   In this scenario, the
 *     function may return (false) without processing the command if
 *     other commands are currently executing; this is to avoid hardware
 *     lockup.
 *
 * @param slot_idx
 *   The slot index with the CONFIGURE_MB command
 * @return
 *   True if CONFIGURE_MB packet was processed, false otherwise
 */
static bool
configure_mb(size_type slot_idx)
{
  CTRL_DEBUG("-->configure_mb\n");
  auto& slot = command_slots[slot_idx];

  CTRL_DEBUGF("configure cmd found in slot(%d)\n",slot_idx);
  slot_size=read_reg(slot.slot_addr + 0x4);
  num_cus=read_reg(slot.slot_addr + 0x8);
  cu_offset=read_reg(slot.slot_addr + 0xC);
  cu_base_address=read_reg(slot.slot_addr + 0x10);

  // Features
  auto features = read_reg(slot.slot_addr + 0x14);
  CTRL_DEBUGF("features=0x%04x\n",features);
  ERT_ASSERT(features & 0x1,"ert is not enabled!!");
  mb_host_interrupt_enabled = (features & 0x2)==0;
  cu_dma_enabled = (features & 0x4)!=0;
  cu_interrupt_enabled = (features & 0x8)!=0;
  cq_status_enabled = (features & 0x10)!=0;
  cdma_enabled = (features & 0x20)!=0;
  dataflow_enabled = (features & 0x40)!=0;
  cu_dma_52 = (features & 0x80000000)!=0;
#ifdef ERT_HW_EMU
  //Force new mechanism for latest emulation platforms
  cu_dma_52 = 1;
#endif
  // CU base address
  for (size_type i=0; i<num_cus; ++i) {
    u32 addr = read_reg(slot.slot_addr + 0x18 + (i<<2));
    cu_addr_map[i] = addr & ~(0xFF); // clear encoded handshake
    CTRL_DEBUGF("cu(%d) @0x%x (0x%x)\n",i,cu_addr_map[i],addr);
  }

  // (Re)initilize MB
  setup();

  // notify host
  notify_host(slot_idx);

  slot.header_value = (slot.header_value & ~0xF) | 0x4; // free
  CTRL_DEBUGF("slot(%d) [running -> free]\n",slot_idx);

  CTRL_DEBUG("<--configure_mb\n");
  return true;
}

static bool
exit_mb(size_type slot_idx)
{
  auto& slot = command_slots[slot_idx];
  CTRL_DEBUGF("exit_mb slot(%d) header=0x%x\n",slot_idx,slot.header_value);

  // disable CUDMA module
  cu_dma_enabled = 0;
  write_reg(ERT_CU_DMA_ENABLE_ADDR,cu_dma_enabled);
  // disable CUISR module
  cu_interrupt_enabled = 0;
  write_reg(ERT_CU_ISR_HANDLER_ENABLE_ADDR,0);

  //Wait for both to go back to IDLE. If system is in bad state we expect host to reset ERT properly
  value_type cu_dma_state = read_reg(ERT_CUDMA_STATE);
  value_type cu_isr_state = read_reg(ERT_CUISR_STATE);
  while(cu_dma_state != ERT_HLS_MODULE_IDLE && cu_isr_state != ERT_HLS_MODULE_IDLE) {
    cu_dma_state = read_reg(ERT_CUDMA_STATE);
    cu_isr_state = read_reg(ERT_CUISR_STATE);
  }

  // Update registers so mgmt driver knows ERT has exited
  slot.header_value = (slot.header_value & ~0xF) | 0x4; // free
  write_reg(slot.slot_addr,slot.header_value); // acknowledge the completed control command
  CTRL_DEBUGF("scheduler loop exits slot(%d) header=0x%x\n",slot_idx,slot.header_value);
  exit(0);
  return true;
}

// Gather ERT stats in ctrl command packet
// [1  ]      : header
// [1  ]      : custat version
// [1  ]      : ert version
// [1  ]      : number of cq slots
// [1  ]      : number of cus
// [#numcus]  : cu execution stats (number of executions)
// [#numcus]  : cu status (1: running, 0: idle)
// [#slots]   : command queue slot status
static bool
cu_stat(size_type slot_idx)
{
  auto& slot = command_slots[slot_idx];
  CTRL_DEBUGF("slot(%d) [new -> queued -> running]\n",slot_idx);
  CTRL_DEBUGF("cu_stat slot(%d) header=0x%x\n",slot_idx,slot.header_value);

  // write stats to command package after header
  size_type pkt_idx = 1; // after header
  size_type max_idx = slot_size >> 2; 

  // custat version, update when changing layout of packet
  write_reg(slot.slot_addr + (pkt_idx++ << 2),0x51a10000);

  // ert git version
  write_reg(slot.slot_addr + (pkt_idx++ << 2),ERT_VERSION);

  // number of cq slots
  write_reg(slot.slot_addr + (pkt_idx++ << 2),num_slots);

  // number of cus
  write_reg(slot.slot_addr + (pkt_idx++ << 2),num_cus);
  
  // individual cu execution stat
  for (size_type i=0; pkt_idx<max_idx && i<num_cus; ++i) {
    CTRL_DEBUGF("cu_usage[0x%x]=%d\n",cu_idx_to_addr(i),cu_usage[i]);
    write_reg(slot.slot_addr + (pkt_idx++ << 2),cu_usage[i]);
  }

  // individual cu status
  for (size_type i=0; pkt_idx<max_idx && i<num_cus; ++i) {
    CTRL_DEBUGF("cu_staus[0x%x]=%d\n",cu_idx_to_addr(i),cu_status[i]);
    write_reg(slot.slot_addr + (pkt_idx++ << 2),cu_status[i]);
  }

  // command slot status
  for (size_type i=0; pkt_idx<max_idx && i<num_slots; ++i) {
    auto& s = command_slots[i];
    CTRL_DEBUGF("slot_status[%d]=%d\n",i,s.header_value & 0XF);
    write_reg(slot.slot_addr + (pkt_idx++ << 2),s.header_value & 0XF);
  }

#if 0
  // payload count
  auto mask = 0X7FF << 12;  // [22-12]
  slot.header_value = (slot.header_value & (~mask)) | (pkt_idx << 12);
  CTRL_DEBUGF("cu_stat new header=0x%x\n",slot.header_value);
  write_reg(slot.slot_addr, slot.header_value);
#endif

  // notify host
  notify_host(slot_idx);
  slot.header_value = (slot.header_value & ~0xF) | 0x4; // free
  CTRL_DEBUGF("slot(%d) [running -> free]\n",slot_idx);
  return true;
}

static bool
abort_mb(size_type slot_idx)
{
  CTRL_DEBUGF("abort cmd found in slot(%d)\n",slot_idx);

  disable_interrupt_guard guard;
  auto& slot = command_slots[slot_idx];
  size_type sidx = (slot.header_value >>  15) & 0xFF;
  auto& s = command_slots[sidx];
  if (opcode(s.header_value)!=ERT_START_KERNEL)
    return true; // bail if not a start_kernel command
  if ((s.header_value & 0xF)!=0x3)
    return true; // bail if not running
  auto cu_idx = s.cu_idx;
  check_command(sidx,cu_idx);
  cu_slot_usage[cu_idx] = no_index;
  cu_status[cu_idx] = !cu_status[cu_idx];
  notify_host(slot_idx);
  return true;
}

/**
 * Process special command.
 *
 * Special commands are not performace critical
 *
 * @return true
 *   If command was processed, false otherwise
 */
static bool
process_special_command(value_type opcode, size_type slot_idx)
{
  if (opcode==ERT_CONFIGURE) // CONFIGURE_MB
    return configure_mb(slot_idx);
  if (opcode==ERT_CU_STAT)
    return cu_stat(slot_idx);
  if (opcode==ERT_EXIT)
    return exit_mb(slot_idx);
  if (opcode==ERT_ABORT)
    return abort_mb(slot_idx);
  return false;
}

/**
 * Transition slot from free to new if possible
 *
 * @return
 *  True if transitioned, false otherwise
 */
inline bool
free_to_new(size_type slot_idx)
{
  // The slot is free and should be updated from command queue
  // only update slot header if host has written 0x1 in header
  auto& slot = command_slots[slot_idx];
  ERT_ASSERT((slot.header_value & 0xF)==0x4,"slot is not free\n");
  auto header =  read_reg(slot.slot_addr);
  if ((header & 0xF) == 0x1) {
    ERT_DEBUGF("new slot(%d)\n",slot_idx);
    write_reg(slot.slot_addr,header | 0xF);
    slot.header_value = header;
    ERT_DEBUGF("slot(%d) [free -> new]\n",slot_idx);
    return true;
  }
  return false;
}

/**
 * Transition slot from new to queued
 *
 * @return
 *   True if command was transitioned, false otherwise.
 */
inline bool
new_to_queued(size_type slot_idx)
{
  auto& slot = command_slots[slot_idx];
  ERT_ASSERT((slot.header_value & 0xF)==0x1,"slot is not new\n");

  auto cmt = cmd_type(slot.header_value);
  auto opc = slot.opcode = opcode(slot.header_value);
  ERT_DEBUGF("slot_idx(%d) type(%d) opcode(%d)\n",slot_idx,cmt,opc);

  if (cmt != ERT_CU) {
    process_special_command(opc,slot_idx);
    return false;
  }

  // new command, gather slot info
  addr_type addr = cu_section_addr(slot.slot_addr);
  slot.cu_idx = read_reg(addr);
  slot.regmap_addr = regmap_section_addr(slot.header_value,slot.slot_addr);
  slot.regmap_size = regmap_size(slot.header_value);
  slot.header_value = (slot.header_value & ~0xF) | 0x2; // queued

  ERT_DEBUGF("slot(%d) [new -> queued]\n",slot_idx);

#ifdef DEBUG_SLOT_STATE
  write_reg(slot.slot_addr,slot.header_value);
#endif

  return true;
}

/**
 * Transition slot from queued to running
 *
 * @return
 *   True if command was transitioned, false otherwise.
 */
inline bool
queued_to_running(size_type slot_idx)
{
  auto& slot = command_slots[slot_idx];
  ERT_ASSERT((slot.header_value & 0xF)==0x2,"slot is not queued\n");

  // disable CU interrupts while starting command
  disable_interrupt_guard guard;
  // queued command, start if cu is ready
  if (start_cu(slot_idx) == no_index)
    return false;

  slot.header_value |= 0x1;       // running (0x2->0x3)
  ERT_DEBUGF("slot(%d) [queued -> running]\n",slot_idx);

#ifdef DEBUG_SLOT_STATE
  write_reg(slot.slot_addr,slot.header_value);
#endif
  return true;
}

/**
 * Transition slot from running to free
 *
 * @return
 *   True if command was transitioned, false otherwise.
 */
inline bool
running_to_free(size_type slot_idx)
{
  auto& slot = command_slots[slot_idx];
  ERT_ASSERT((slot.header_value & 0xF)==0x3,"slot is not running\n");
  
  // running command, check its cu status
  if (!check_cu(slot.cu_idx))
    return false;

  notify_host(slot_idx);
  slot.header_value = (slot.header_value & ~0xF) | 0x4; // free
  ERT_DEBUGF("slot(%d) [running -> free]\n",slot_idx);

#ifdef DEBUG_SLOT_STATE
  write_reg(slot.slot_addr,slot.header_value);
#endif
  return true;
}

/**
 * Main routine executed by embedded scheduler loop
 *
 * For each command slot do
 *  1. If status is free (0x4), then read new command header
 *     Status remains free (0x4), or transitions to new (0x1)
 *  2. If status is new (0x1), then read CUs in command
 *     Status transitions to queued (0x2)
 *  3. If status is queued (0x2), then start command on available CU
 *     Status remains queued if no CUs available, or transitions to running (0x3)
 *  4. If status is running (0x4), then check CU status
 *     Status remains running (0x4) if CU is still running, or
 *     transitions to free if CU is done
 */
static void
scheduler_loop()
{
  ERT_DEBUG("ERT scheduler\n");

  // Set up ERT base address, this should only call once
  setup_ert_base_addr();

  // Basic setup will be changed by configure_mb, but is necessary
  // for even configure_mb() to work.
  setup();

  while (1) {
    for (size_type slot_idx=0; slot_idx<num_slots; ++slot_idx) {
      auto& slot = command_slots[slot_idx];

#ifdef ERT_HW_EMU
      reg_access_wait();
#endif

      // In dataflow mode ERT is polling CUs for completion after
      // host has started CU or acknowleged completion.  Ctrl cmds
      // are processed in normal flow.
      if (dataflow_enabled && slot_idx>0) {
        size_type cuidx = slot_idx-1;  // compensate for reserved slot (0)

        // Check if host has started or continued this CU
        if (!cu_status[cuidx]) {
          auto cqvalue = read_reg(slot.slot_addr);
          if (cqvalue & (AP_START|AP_CONTINUE)) {
            write_reg(slot.slot_addr,0x0); // clear
            ERT_DEBUGF("enable cu(%d) cqvalue(0x%x)\n",cuidx,cqvalue);
            cu_status[cuidx] = !cu_status[cuidx]; // enable polling of this CU
          }
        }

        if (!cu_status[cuidx])
          continue; // CU is not used

        auto cuvalue = read_reg(cu_idx_to_addr(cuidx));
        if (!(cuvalue & AP_DONE))
          continue;

        cu_status[cuidx] = !cu_status[cuidx]; // disable polling until host re-enables
        ERT_DEBUGF("polled cu(%d) cuvalue(0x%x)\n",cuidx,cuvalue);

        // wake up host
        notify_host(slot_idx);
        continue;
      }

      // CQ_STATUS_ENABLED CHECK WON'T WORK IF HOST TRANSITIONS
      // FROM ENABLED -> DISABLED IN CONFIGURE COMMAND
      if (!cq_status_enabled && ((slot.header_value & 0xF) == 0x4)) { // free
        if (!free_to_new(slot_idx))
          continue;
      }

      if ((slot.header_value & 0xF) == 0x1) { // new
        if (!new_to_queued(slot_idx))
          continue;
      }

      if ((slot.header_value & 0xF) == 0x2) { // queued
        if (!queued_to_running(slot_idx))
          continue;
      }

      if (!cu_interrupt_enabled && ((slot.header_value & 0xF) == 0x3)) { // running
        if (!running_to_free(slot_idx))
          continue;
      }
    }
  } // while
}

/**
 * CU interrupt service routine
 */
#ifndef ERT_HW_EMU
void cu_interrupt_handler() __attribute__((interrupt_handler));
#endif
void
cu_interrupt_handler()
{
  ERT_DEBUG("interrupt_handler\n");
  bitmask_type intc_mask = read_reg(ERT_INTC_IPR_ADDR);

  if (intc_mask & 0x2) { // cuisr interrupt
    for (size_type w=0,offset=0; w<num_cu_masks; ++w,offset+=32) {
      auto cu_mask = read_reg(CU_STATUS_REGISTER_ADDR[w]);
      for (size_type cu_idx=offset; cu_mask; cu_mask >>= 1, ++cu_idx) {
        if ((cu_mask & 0x1) && cu_interrupt_mask[cu_idx]) {
          ERT_DEBUGF("cu(%d) is interrupting\n",cu_idx);
          ERT_ASSERT(cu_status[cu_idx],"cu wasn't started");
          // check if command is done
          check_command(cu_slot_usage[cu_idx],cu_idx);
          cu_slot_usage[cu_idx] = no_index; // reset slot index
          cu_status[cu_idx] = !cu_status[cu_idx]; // toggle status of completed cus
        }
      }
    }
  }

  if (intc_mask & 0x1) { // host interrupt
    for (size_type w=0,offset=0; w<num_slot_masks; ++w,offset+=32) {
      auto slot_mask = read_reg(CQ_STATUS_REGISTER_ADDR[w]);
      ERT_DEBUGF("command queue interrupt from host: 0x%x\n",slot_mask);
      // Transition each new command into new state
      for (size_type slot_idx=offset; slot_mask; slot_mask >>= 1, ++slot_idx)
        if (slot_mask & 0x1)
          free_to_new(slot_idx);
    }
  }

  if (intc_mask & 0x4) { // cdma interrupt
    auto cu_idx = num_cus-1; // cdma is last cu
    ERT_DEBUGF("cdma cu(%d) interrupts\n",cu_idx);
    ERT_ASSERT(cu_status[cu_idx],"cdma cu wasn't started");
    check_command(cu_slot_usage[cu_idx],cu_idx);
    cu_slot_usage[cu_idx] = no_index; // reset slot index;
    cu_status[cu_idx] = !cu_status[cu_idx]; // toggle status of completed cus

    // Reset cdma (1) read status to clear it, (2) reset isr at base + 0xC
    ERT_UNUSED volatile auto val = read_reg(cu_idx_to_addr(cu_idx));
    write_reg(cu_idx_to_addr(cu_idx) + 0xC,1);
  }

  // Acknowledge interrupts
  write_reg(ERT_INTC_IAR_ADDR,intc_mask);
}

} // ert
#ifdef ERT_HW_EMU
#ifdef __cplusplus
extern "C" {
#endif
void scheduler_loop() {
    ert::scheduler_loop();
}

void cu_interrupt_handler() {
    ert::cu_interrupt_handler();
}

#ifdef __cplusplus
}
#endif
#endif

#ifndef ERT_HW_EMU
int main()
{
  ert::scheduler_loop();
  return 0;
}
#endif
