--------------------------------------------------------------------------------
-- CERN BE-CO-HT
-- Mock Turtle
-- https://gitlab.cern.ch/coht/mockturtle
--------------------------------------------------------------------------------
--
-- unit name:   mt_urv_wrapper
--
-- description: A small wrapper for the URV encompassing the internal RAM and
-- access to the RAM through CPU CSR register block.
--
--------------------------------------------------------------------------------
-- Copyright CERN 2014-2018
--------------------------------------------------------------------------------
-- Copyright and related rights are licensed under the Solderpad Hardware
-- License, Version 2.0 (the "License"); you may not use this file except
-- in compliance with the License. You may obtain a copy of the License at
-- http://solderpad.org/licenses/SHL-2.0.
-- Unless required by applicable law or agreed to in writing, software,
-- hardware and materials distributed under this 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.
--------------------------------------------------------------------------------

library ieee;
use ieee.std_logic_1164.all;
use ieee.numeric_std.all;

use work.genram_pkg.all;
--use work.memory_loader_pkg.all;
use work.wishbone_pkg.all;
use work.urv_pkg.all;
use work.secded_32b_pkg.all;

entity hydra_core is
  generic(
    g_IRAM_LOG_SIZE : natural := 12;  --  In bytes
    g_DRAM_LOG_SIZE : natural := 12;
    g_TRIPLE_CORE   : boolean := True;
    g_SIM_SEU_PERIOD : natural := 0);
  port(
    clk_sys_i   : in  std_logic;
    clk_cpu_i   : in  std_logic_vector(3 downto 1);
    --  Reset the whole logic.
    rst_n_i     : in  std_logic;
    --  Reset only the cpu + clear ram if requested.
    --  To be used in case of uncorrectable error.
    --  NOT to be used for recovery.
    cpu_rst_n_i : in  std_logic;

    --  For peripherals
    dwb_o     : out t_wishbone_master_out;
    dwb_i     : in  t_wishbone_master_in;

    --  IRAM write access (during reset)
    iram_addr_i : in  std_logic_vector(g_IRAM_LOG_SIZE - 1 downto 2);
    iram_we_i   : in  std_logic;
    iram_data_i : in  std_logic_vector(31 downto 0);
    iram_ecc_i  : in  std_logic_vector(6 downto 0);

    -- Set to clear DRAM while cpu reset.
    dram_clr_i  : in  std_logic;

    --  A fault has been detected, CPUs are reset.
    cpu_flt_o : out std_logic;
    --  A reset (recovery) has been requested, CPUs are reset.
    --  To be used only for statistics.
    cpu_rst_o : out std_logic;

    --  Supervisor registers visible to the outside
    reset_cause_o : out std_logic_vector(31 downto 0);
    nbr_iram_ecc_corr_o : out std_logic_vector(31 downto 0);
    nbr_iram_scrub_corr_o : out std_logic_vector(31 downto 0);
    nbr_iram_scrub_uncorr_last_o : out std_logic_vector(15 downto 0);
    nbr_iram_scrub_cycle_o : out std_logic_vector(15 downto 0);
    iram_scrub_en_o : out std_logic;
    nbr_dram_ecc_corr_o : out std_logic_vector(31 downto 0);
    nbr_dram_scrub_corr_o : out std_logic_vector(31 downto 0);
    nbr_dram_scrub_uncorr_last_o : out std_logic_vector(15 downto 0);
    nbr_dram_scrub_cycle_o : out std_logic_vector(15 downto 0);
    dram_scrub_en_o : out std_logic;
    nbr_cpu_fault_o : out std_logic_vector(31 downto 0)
  );
end hydra_core;

architecture arch of hydra_core is

  impure function f_x_to_zero (x : std_logic_vector) return std_logic_vector
  is
    variable tmp : std_logic_vector(x'length-1 downto 0);
    variable found_undef : boolean := false;
  begin
-- synthesis translate_off
    for i in 0 to x'length-1 loop
      if( x(i) = 'U' or x(i) = 'Z' or x(i) = 'X' ) then
        found_undef := true;
      end if;

      if x(i) = '1' or x(i) = 'H' then
        tmp(i) := '1';
      else
        tmp(i) := '0';
      end if;
    end loop;
    return tmp;

    if found_undef then
      report "Undefined data value read from memory" severity warning;
    end if;

-- synthesis translate_on
    return x;
  end function f_x_to_zero;

  signal cpu_rst, cpu_rst2        : std_logic_vector(1 to 3);
  signal cpu_rst_err, cpu_flt    : std_logic;
  signal cpu_sync       : std_logic_vector(1 to 3);

  signal im_addr  : std_logic_vector(31 downto 0);
  signal im_data, im1_data  : std_logic_vector(31 downto 0);
  signal im_rd, im_valid : std_logic;
  signal im1_done, im1_dm_en, im1_err : std_logic;
  signal im2_err, im2_valid : std_logic;
  signal err_cpu_im, err_cpu_dm : std_logic;

  signal dm_addr, dm_data_s, dm_data_l       : std_logic_vector(31 downto 0);
  signal dm_data_select                      : std_logic_vector(3 downto 0);
  signal dm_load, dm_store, dm_load_done, dm_store_done : std_logic;

  signal reg_dm_addr, reg_dm_data_s : std_logic_vector(31 downto 0);
  signal reg_dm_data_select         : std_logic_vector(3 downto 0);
  signal reg_dm_load, reg_dm_store  : std_logic;
  signal reg_dm_en : std_logic;

  signal dm_cycle_in_progress : std_logic;
  signal reg_dm_is_wishbone, reg_dm_is_reg : std_logic;
  signal reg_dm_is_ram, reg_dm_is_iram, reg_dm_is_dram : std_logic;

  signal dm_mem_rdata, dm_wb_rdata : std_logic_vector(31 downto 0);
  signal dm_wb_write, dm_select_wb, dm_select_ram : std_logic;
  signal dm_done, dm_err : std_logic;
  signal dram_ecc_mask : std_logic_vector(7 downto 0);

  signal dwb_out         : t_wishbone_master_out;

  signal sv_wb_out         : t_wishbone_master_out;
  signal sv_wb_in         : t_wishbone_master_in;

  signal nbr_iram_ecc_corr   : std_logic_vector (31 downto 0);
  signal nbr_dram_ecc_corr   : std_logic_vector (31 downto 0);
  signal nbr_iram_scrub_corr   : std_logic_vector (31 downto 0);
  signal nbr_dram_scrub_corr   : std_logic_vector (31 downto 0);
  signal nbr_iram_scrub_uncorr_curr, nbr_iram_scrub_uncorr_last : std_logic_vector (15 downto 0);
  signal nbr_dram_scrub_uncorr_curr, nbr_dram_scrub_uncorr_last : std_logic_vector (15 downto 0);
  signal nbr_iram_scrub_cycle  : std_logic_vector (15 downto 0);
  signal nbr_dram_scrub_cycle  : std_logic_vector (15 downto 0);
  signal iram_ecc_err, iram_ecc_fatal : std_logic;
  signal iram_scrub_se, iram_scrub_de : std_logic;
  signal dram_ecc_err, dram_ecc_fatal : std_logic;
  signal dram_scrub_se, dram_scrub_de : std_logic;
  signal iram_scrub_en, dram_scrub_en : std_logic;
  signal iram_scrub_act, dram_scrub_act : std_logic;
  signal iram_scrub_cycle, dram_scrub_cycle : std_logic;
  signal scrub_cfg_wr, scrub_cfg_iram_val, scrub_cfg_dram_val : std_logic;
  signal nbr_cpu_data_err : std_logic_vector (31 downto 0);
  signal nbr_cpu_iaddr_err : std_logic_vector (31 downto 0);
  signal iram_ecc_addr : std_logic_vector (31 downto 0) := (others => '0');

  signal reset_cause_cpu, reset_cause_wd : std_logic;
  signal reset_cause_iram_ecc, reset_cause_dram_ecc : std_logic;
  signal reset_cause_recovery : std_logic;
  signal dram_scrub_period, iram_scrub_period : std_logic_vector(15 downto 0);

  --  Initial watchdog period.
  constant wd_init : std_logic_vector(31 downto 0) := x"0000_07ff";

  signal wd_period, wd_period_val, wd_counter, wd_key_val : std_logic_vector(31 downto 0);
  signal wd_period_wr, wd_key_wr, wd_timeout : std_logic;

  signal force_divergence, force_divergence_d, dm_force_divergence : std_logic;
  signal cpu_wr : std_logic;

  type t_state is (S_VOTER, S_LOCK);
  signal state : t_state;

  signal cpu_recovery, cpu_recovery_in, cpu_recovery_wr : std_logic;

  signal iram_err_addr       : std_logic_vector(g_IRAM_LOG_SIZE - 1 downto 2);
  signal iram_err_addr_en, iram_err_addr_en_out  : std_logic;
  signal iram_err_addr_done  : std_logic;
  signal iram_err_addr_in, iram_err_addr_out    : std_logic_vector(15 downto 2);
  signal iram_err_addr_wr    : std_logic;

  signal cpu_fault_count : std_logic_vector(31 downto 0);
  signal cpu_fault_count_wr : std_logic;
begin
  dwb_o <= dwb_out;

  cpu_rst2 <= "111" when rst_n_i = '0' or cpu_rst_n_i = '0' else cpu_rst;
  
  gen_triple_cpu: if g_TRIPLE_CORE generate
    inst_cpus : entity work.hydra_triple_cpu
      port map (
        clk_i            => clk_sys_i,
        clk_cpu_i        => clk_cpu_i,
        cpu_rst_i        => cpu_rst2,
        im_addr_o        => im_addr,
        im_rd_o          => im_rd,
        im_data_i        => im_data,
        im_valid_i       => im_valid,
        dm_addr_o        => dm_addr,
        dm_data_s_o      => dm_data_s,
        dm_data_l_i      => dm_data_l,
        dm_data_select_o => dm_data_select,
        dm_store_o       => dm_store,
        dm_load_o        => dm_load,
        dm_load_done_i   => dm_load_done,
        dm_store_done_i  => dm_store_done,

        cpu_sync_o       => cpu_sync,
        dm_force_divergence_i => dm_force_divergence,
        err_cpu_im_o     => err_cpu_im,
        err_cpu_dm_o     => err_cpu_dm);
  end generate;

  gen_one_cpu: if not g_TRIPLE_CORE generate
    signal fault : std_logic;
  begin
    inst_cpu : urv_cpu
    generic map (
      g_timer_frequency => 0,
      g_with_hw_debug => 0,
      g_with_hw_mul => 0,
      g_with_hw_div => 0,
      g_with_ecc => 1
      )
    port map (
      clk_i            => clk_sys_i,
      rst_i            => cpu_rst2(1),
      irq_i            => '0',
      fault_o          => fault,
      im_addr_o        => im_addr,
      im_rd_o          => im_rd,
      im_data_i        => im_data,
      im_valid_i       => im_valid,
      dm_addr_o        => dm_addr,
      dm_data_s_o      => dm_data_s,
      dm_data_l_i      => dm_data_l,
      dm_data_select_o => dm_data_select,
      dm_store_o       => dm_store,
      dm_load_o        => dm_load,
      dm_load_done_i   => dm_load_done,
      dm_store_done_i  => dm_store_done,
      dbg_force_i      => '0',
      dbg_enabled_o    => open,
      dbg_insn_i       => x"0000_0000",
      dbg_insn_set_i   => '0',
      dbg_insn_ready_o => open,
      dbg_mbx_data_i   => x"0000_0000",
      dbg_mbx_write_i  => '0',
      dbg_mbx_data_o   => open);
    cpu_sync <= (1 to 3 => not fault);
    err_cpu_im <= '0';
    err_cpu_dm <= '0';
  end generate;

  --  Add registers on uRV data bus
  process (clk_sys_i)
  begin
    if rising_edge(clk_sys_i) then
      if rst_n_i = '0' then
        reg_dm_addr <= x"0004_0000";
        reg_dm_data_s <= (others => '0');
        reg_dm_data_select <= (others => '1');
      elsif cpu_rst_n_i = '0' then
        --  Clear dram.
        if dm_done = '1' then
          reg_dm_addr <= std_logic_vector(unsigned(reg_dm_addr) + 4);
        end if;
      else
        if dm_load = '1' or dm_store = '1' then
          reg_dm_addr <= dm_addr;
          reg_dm_data_s <= dm_data_s;
          reg_dm_data_select <= dm_data_select;
        end if;
      end if;
    end if;
  end process;

  --  No scrubbing during reset (as the init code also writes)
  iram_scrub_act <= iram_scrub_en and cpu_rst_n_i;

  inst_iram: entity work.hydra_iram
    generic map (
      g_RAM_LOG_SIZE => g_IRAM_LOG_SIZE,
      g_SIM_SEU_PERIOD => g_SIM_SEU_PERIOD
    )
    port map (
      clk_i => clk_sys_i,
      rst_n_i => rst_n_i,
      scrub_en_i => iram_scrub_act,

      --  uRV data read from iRAM
      r1_addr_i => reg_dm_addr(g_IRAM_LOG_SIZE - 1 downto 2),
      r1_en_i   => im1_dm_en,
      r1_data_o => im1_data,
      r1_done_o => im1_done,
      r1_err_o  => im1_err,

      --  uRV instructions
      r2_addr_i => im_addr(g_IRAM_LOG_SIZE - 1 downto 2),
      r2_en_i   => im_rd,
      r2_data_o => im_data,
      r2_done_o => im2_valid,
      r2_err_o  => im2_err,

      --  iRAM initialization
      waddr_i => iram_addr_i,
      we_i => iram_we_i,
      wdata_i => iram_data_i,
      wecc_i => iram_ecc_i,
      wdone_o => open,

      ecc_one_o => iram_ecc_err,
      ecc_fatal_o => iram_ecc_fatal,
      ecc_addr_o => iram_ecc_addr(g_IRAM_LOG_SIZE - 1 downto 2),

      err_addr_i => iram_err_addr,
      err_addr_en_i => iram_err_addr_en,
      err_addr_done_o => iram_err_addr_done,

      scrub_se_o => iram_scrub_se,
      scrub_de_o => iram_scrub_de,
      scrub_cycle_o => iram_scrub_cycle,
      scrubber_period_i => iram_scrub_period
    );

  im_valid <= im2_valid and not im2_err;

  -- Memory map:
  -- 0     - 256KB: IRAM
  -- 256KB - 512kB: DRAM
  -- 512KB -   1MB: Supervisor
  --   1MB -      : periperal
  reg_dm_is_wishbone <= '1' when reg_dm_addr(31 downto 20) /= x"000" else '0';
  reg_dm_is_ram <= '1' when reg_dm_addr(19) = '0' and reg_dm_is_wishbone = '0' else '0';
  reg_dm_is_reg <= '1' when reg_dm_addr(19) = '1' and reg_dm_is_wishbone = '0' else '0';
  reg_dm_is_iram <= '1' when reg_dm_addr(18) = '0' and reg_dm_is_ram = '1' else '0';
  reg_dm_is_dram <= '1' when reg_dm_addr(18) = '1' and reg_dm_is_ram = '1' else '0';
  im1_dm_en <= reg_dm_load and reg_dm_is_iram;
  dm_data_l <= dm_wb_rdata;

  reg_dm_en <= '1' when reg_dm_is_dram = '1' and (reg_dm_store or reg_dm_load) = '1' else '0';

  dram_scrub_act <= dram_scrub_en and cpu_rst_n_i;

  inst_dram: entity work.hydra_dram
    generic map (
      g_ram_log_size => g_dram_log_size,
      g_SIM_SEU_PERIOD => g_SIM_SEU_PERIOD
    )
    port map (
      clk_i => clk_sys_i,
      rst_n_i => rst_n_i,
      scrub_en_i => dram_scrub_act,
      addr_i => reg_dm_addr(g_DRAM_LOG_SIZE - 1 downto 2),
      en_i => reg_dm_en,
      we_i => reg_dm_store,
      sel_i => reg_dm_data_select,
      data_i => reg_dm_data_s,
      eccm_i => dram_ecc_mask(6 downto 0),
      data_o => dm_mem_rdata,
      done_o => dm_done,
      err_o => dm_err,
      ecc_one_o => dram_ecc_err,
      ecc_fatal_o => dram_ecc_fatal,
      scrub_se_o => dram_scrub_se,
      scrub_de_o => dram_scrub_de,
      scrub_cycle_o => dram_scrub_cycle,
      scrubber_period_i => dram_scrub_period
    );

  -- Data bus
  -- Wishbone bus arbitration / internal RAM access
  p_wishbone_master : process(clk_sys_i)
  begin
    if rising_edge(clk_sys_i) then
      dm_store_done <= '0';
      dm_load_done  <= '0';
      dm_force_divergence <= '0';
      force_divergence_d <= force_divergence;

      if rst_n_i = '0' then
        dwb_out.cyc          <= '0';
        dwb_out.stb          <= '0';
        dwb_out.adr          <= (others => '0');
        dwb_out.sel          <= x"0";
        dwb_out.we           <= '0';
        dwb_out.dat          <= (others => '0');
        dm_cycle_in_progress <= '0';
        dm_select_wb         <= '0';
        sv_wb_out.cyc        <= '0';
        sv_wb_out.stb        <= '0';
        reg_dm_load          <= '0';
        reg_dm_store         <= '0';
      elsif cpu_rst_n_i = '0' and dram_clr_i = '1' then
        --  Write to dram until the last address.
        reg_dm_store <= not reg_dm_addr(g_DRAM_LOG_SIZE);
      else
        if dm_cycle_in_progress = '0' then
          --  Data bus was idle.
          if dm_load = '1' then
            reg_dm_load <= '1';
          end if;
          if dm_store = '1' then
            reg_dm_store <= '1';
          end if;

          if reg_dm_load = '1' or reg_dm_store = '1' then
            dm_wb_write  <= reg_dm_store;
            if reg_dm_is_ram = '1' then
              --  Internal access
              dm_select_wb  <= '0';
              dm_select_ram <= '1';
              --  Need to wait for the done (may need extra cycles due to ECC)
              dm_cycle_in_progress <= '1';
              --  TODO: detect write access to IRAM ?
            else
              --  Wishbone access
              dm_select_ram <= '0';
              dm_select_wb   <= reg_dm_is_wishbone;
              dwb_out.cyc    <= reg_dm_is_wishbone;
              dwb_out.stb    <= reg_dm_is_wishbone;
              dwb_out.we     <= reg_dm_store;
              dwb_out.adr    <= reg_dm_addr;
              dwb_out.dat    <= reg_dm_data_s;
              dwb_out.sel    <= reg_dm_data_select;
              sv_wb_out.cyc  <= reg_dm_is_reg;
              sv_wb_out.stb  <= reg_dm_is_reg;
              sv_wb_out.we   <= reg_dm_store;
              sv_wb_out.adr  <= reg_dm_addr;
              sv_wb_out.dat  <= reg_dm_data_s;
              sv_wb_out.sel  <= reg_dm_data_select;
              dm_cycle_in_progress <= '1';
            end if;
          end if;
        else
          --  Transfer in progress
          --  Cannot start a new transfer.
          assert dm_load = '0';
          assert dm_store = '0';
          if dm_select_ram = '0' then
            --  Wishbone transfer in progress.
            if dm_select_wb = '1' and dwb_i.stall = '0' then
              dwb_out.stb <= '0';
            end if;
            if (dm_select_wb = '1' and dwb_i.ack = '1')
              or (dm_select_wb = '0' and sv_wb_in.ack = '1')
            then
              if dm_wb_write = '0' then
                --  Read acked
                dm_load_done <= '1';
                reg_dm_load <= '0';
                if dm_select_wb = '1' then
                  --  from peripheral
                  dm_wb_rdata  <= f_x_to_zero(dwb_i.dat);
                else
                  --  from supervisor
                  dm_wb_rdata <= sv_wb_in.dat;
                  dm_force_divergence <= force_divergence_d;
                end if;
              else
                --  Write acked
                dm_store_done <= '1';
                reg_dm_store <= '0';
              end if;

              dm_cycle_in_progress <= '0';
              if dm_select_wb = '1' then
                dwb_out.cyc          <= '0';
              else
                sv_wb_out.stb <= '0';
                sv_wb_out.cyc <= '0';
              end if;
            end if;
          else
            --  IRAM/DRAM transfer in progress
            if reg_dm_is_iram = '1' then
              if im1_done = '1' and im1_err = '0' then
                dm_wb_rdata <= im1_data;
                dm_load_done <= '1';
                reg_dm_load <= '0';
                dm_cycle_in_progress <= '0';
              end if;
            else
              if dm_done = '1' and dm_err = '0' then
                dm_wb_rdata <= dm_mem_rdata;
                dm_cycle_in_progress <= '0';
                if dm_wb_write = '0' then
                  dm_load_done <= '1';
                  reg_dm_load <= '0';
                else
                  dm_store_done <= '1';
                  reg_dm_store <= '0';
                end if;
              end if;
            end if;
          end if;
        end if;
      end if;
    end if;
  end process p_wishbone_master;

  --  Stats
  p_ecc_stats: process (clk_sys_i)
  begin
    if rising_edge(clk_sys_i) then
      if rst_n_i = '0' then
        nbr_iram_ecc_corr <= (others => '0');
        nbr_dram_ecc_corr <= (others => '0');
        nbr_iram_scrub_corr <= (others => '0');
        nbr_dram_scrub_corr <= (others => '0');
        nbr_iram_scrub_uncorr_curr <= (others => '0');
        nbr_iram_scrub_uncorr_last <= (others => '0');
        nbr_iram_scrub_cycle <= (others => '0');
        nbr_dram_scrub_uncorr_curr <= (others => '0');
        nbr_dram_scrub_uncorr_last <= (others => '0');
        nbr_dram_scrub_cycle <= (others => '0');
      else
        if iram_ecc_err = '1' then
          nbr_iram_ecc_corr <= std_logic_vector(unsigned(nbr_iram_ecc_corr) + 1);
        end if;
        if iram_scrub_se = '1' then
          nbr_iram_scrub_corr <= std_logic_vector(unsigned(nbr_iram_scrub_corr) + 1);
        end if;
        if iram_scrub_de = '1' and nbr_iram_scrub_uncorr_curr(15) /= '1' then
          nbr_iram_scrub_uncorr_curr <= std_logic_vector(unsigned(nbr_iram_scrub_uncorr_curr) + 1);
        end if;
        if iram_scrub_cycle = '1' then
          nbr_iram_scrub_uncorr_last <= nbr_iram_scrub_uncorr_curr;
          nbr_iram_scrub_uncorr_curr <= (others => '0');
          nbr_iram_scrub_cycle <= std_logic_vector(unsigned(nbr_iram_scrub_cycle) + 1);
        end if;
        if dram_ecc_err = '1' then
          nbr_dram_ecc_corr <= std_logic_vector(unsigned(nbr_dram_ecc_corr) + 1);
        end if;
        if dram_scrub_se = '1' then
          nbr_dram_scrub_corr <= std_logic_vector(unsigned(nbr_dram_scrub_corr) + 1);
        end if;
        if dram_scrub_de = '1' and nbr_dram_scrub_uncorr_curr(15) /= '1' then
          nbr_dram_scrub_uncorr_curr <= std_logic_vector(unsigned(nbr_dram_scrub_uncorr_curr) + 1);
        end if;
        if dram_scrub_cycle = '1' then
          nbr_dram_scrub_uncorr_last <= nbr_dram_scrub_uncorr_curr;
          nbr_dram_scrub_uncorr_curr <= (others => '0');
          nbr_dram_scrub_cycle <= std_logic_vector(unsigned(nbr_dram_scrub_cycle) + 1);
        end if;
      end if;
    end if;
  end process;

  --  Watchdog
  p_watchdog: process (clk_sys_i)
  begin
    if rising_edge(clk_sys_i) then
      if rst_n_i = '0' then
        wd_period <= wd_init;
        wd_counter <= wd_init;
        wd_timeout <= '0';
      else
        wd_timeout <= '0';
        if wd_period_wr = '1' then
          --  Period change
          --  Note: counter doesn't change.
          wd_period <= wd_period_val;
        else
          if wd_counter = (wd_counter'range => '0') then
            --  Timeout
            wd_timeout <= '1';
            --  Use the initial (small) period
            wd_counter <= wd_init;
          elsif cpu_rst = "111" or (wd_key_wr = '1' and wd_key_val = x"c0423bc9") then
            --  Key -> reload
            --  Also restart the watchdog if the cpu is reset.
            wd_counter <= wd_period;
          else
            --  Decrement the watchdog
            wd_counter <= std_logic_vector(unsigned(wd_counter) - 1);
          end if;
        end if;
      end if;
    end if;
  end process;

  --  Scrubbers enable
  p_scrub_cfg: process (clk_sys_i)
  begin
    if rising_edge(clk_sys_i) then
      if rst_n_i = '0' then
        iram_scrub_en <= '0';
        dram_scrub_en <= '0';
      elsif scrub_cfg_wr = '1' then
        iram_scrub_en <= iram_scrub_en or scrub_cfg_iram_val;
        dram_scrub_en <= dram_scrub_en or scrub_cfg_dram_val;
      end if;
    end if;
  end process;

  --  Any fatal error that should reset cpus
  cpu_rst_err <= dram_ecc_fatal or iram_ecc_fatal or wd_timeout;
  cpu_flt <= '1' when cpu_sync = "000" or cpu_rst_err = '1' else '0';

  inst_sv_regs: entity work.hydra_supervisor_regs
    port map (
      rst_n_i => rst_n_i,
      clk_i => clk_sys_i,
      wb_i => sv_wb_out,
      wb_o => sv_wb_in,
      reset_cause_cpu_i => reset_cause_cpu,
      reset_cause_iram_ecc_i => reset_cause_iram_ecc,
      reset_cause_dram_ecc_i => reset_cause_dram_ecc,
      reset_cause_watchdog_i => reset_cause_wd,
      reset_cause_recovery_i => reset_cause_recovery,
      cpu_reset_i => cpu_rst,
      cpu_reset_o => open,
      cpu_recovery_i => cpu_recovery,
      cpu_recovery_o => cpu_recovery_in,
      cpu_wr_o => cpu_wr,
      force_divergence_i => x"00000000",
      force_divergence_rd_o => force_divergence,
      wd_period_i => wd_period,
      wd_period_o => wd_period_val,
      wd_period_wr_o => wd_period_wr,
      wd_count_i => wd_counter,
      wd_key_o => wd_key_val,
      wd_key_wr_o => wd_key_wr,
      iram_ecc_se_i => nbr_iram_ecc_corr,
      iram_scrub_se_i => nbr_iram_scrub_corr,
      iram_scrub_de_curr_i => nbr_iram_scrub_uncorr_curr,
      iram_scrub_de_last_i => nbr_iram_scrub_uncorr_last,
      iram_scrub_cycle_i => nbr_iram_scrub_cycle,
      dram_ecc_se_i => nbr_dram_ecc_corr,
      dram_scrub_se_i => nbr_dram_scrub_corr,
      dram_scrub_de_curr_i => nbr_dram_scrub_uncorr_curr,
      dram_scrub_de_last_i => nbr_dram_scrub_uncorr_last,
      dram_scrub_cycle_i => nbr_dram_scrub_cycle,
      cpu_data_err_i => nbr_cpu_data_err,
      cpu_iaddr_err_i => nbr_cpu_iaddr_err,
      dram_ecc_mask_o => dram_ecc_mask,
      iram_ecc_addr_i => iram_ecc_addr,
      iram_err_addr_addr_i => iram_err_addr_in,
      iram_err_addr_addr_o => iram_err_addr_out,
      iram_err_addr_en_i => iram_err_addr_en,
      iram_err_addr_en_o => iram_err_addr_en_out,
      iram_err_addr_wr_o => iram_err_addr_wr,
      scrub_cfg_iram_en_i => iram_scrub_en,
      scrub_cfg_iram_en_o => scrub_cfg_iram_val,
      scrub_cfg_dram_en_i => dram_scrub_en,
      scrub_cfg_dram_en_o => scrub_cfg_dram_val,
      scrub_cfg_wr_o      => scrub_cfg_wr,
      iram_scrub_period_o => iram_scrub_period,
      dram_scrub_period_o => dram_scrub_period,
      cpu_fault_count_i => cpu_fault_count,
      cpu_fault_count_wr_o => cpu_fault_count_wr,
      cpu_fault_count_o => open
    );

  cpu_recovery_wr <= cpu_wr and cpu_recovery_in;

  p_cpu_fault_count: process (clk_sys_i) is
  begin
    if rising_edge(clk_sys_i) then
      if rst_n_i = '0' then
        cpu_fault_count <= (others => '0');
      elsif cpu_fault_count_wr = '1' then
        cpu_fault_count <= std_logic_vector(unsigned (cpu_fault_count) + 1);
      end if;
    end if;
  end process;

  nbr_cpu_fault_o <= cpu_fault_count;
  
  iram_err_addr_in (g_IRAM_LOG_SIZE - 1 downto 2) <= iram_err_addr;
  iram_err_addr_in (15 downto g_IRAM_LOG_SIZE) <= (others => '0');

  p_iram_err: process (clk_sys_i) is
  begin
    if rising_edge(clk_sys_i) then
      if rst_n_i = '0' then
        iram_err_addr_en <= '0';
        iram_err_addr <= (others => '0');
      elsif iram_err_addr_wr = '1' then
        iram_err_addr_en <= iram_err_addr_en_out;
        iram_err_addr <= iram_err_addr_out (g_IRAM_LOG_SIZE - 1 downto 2);
      elsif iram_err_addr_done = '1' then
        iram_err_addr_en <= '0';
      end if;
    end if;
  end process;

  p_reset: process (clk_sys_i) is
  begin
    if rising_edge(clk_sys_i) then
      cpu_rst_o <= '0';
      cpu_flt_o <= '0';

      if rst_n_i = '0' or cpu_rst_n_i = '0' then
        state <= S_VOTER;
        cpu_rst <= (others => '1');
        if rst_n_i = '0' then
          cpu_recovery <= '0';
          nbr_cpu_data_err <= (others => '0');
          nbr_cpu_iaddr_err <= (others => '0');
          reset_cause_cpu <= '0';
          reset_cause_iram_ecc <= '0';
          reset_cause_dram_ecc <= '0';
          reset_cause_wd <= '0';
          reset_cause_recovery <= '0';
        end if;
      else
        case state is
          when S_VOTER =>
            if dm_cycle_in_progress = '0' then
              --  Release reset only when the bus is idle.
              cpu_rst <= "000";
            end if;
            --  Software can clear recovery flag.
            if cpu_wr = '1' and cpu_recovery_in = '0' then
              cpu_recovery <= '0';
            end if;
            --  Data bus errors counter
            if err_cpu_dm = '1' then
              nbr_cpu_data_err <= std_logic_vector(unsigned(nbr_cpu_data_err) + 1);
            end if;
            if err_cpu_im = '1' then
              nbr_cpu_iaddr_err <= std_logic_vector(unsigned(nbr_cpu_iaddr_err) + 1);
            end if;
            if cpu_sync = "110" or cpu_sync = "101" or cpu_sync = "011" then
              --  Disable the cpu out of sync.
              cpu_rst <= not cpu_sync;
              --  Go to lock-step
              state <= S_LOCK;
            end if;
          when S_LOCK =>
            if cpu_recovery_wr = '1' then
              --  Start recovery (reset)
              cpu_recovery <= '1';
            end if;
        end case;

        if cpu_flt = '1' or cpu_recovery_wr = '1' then
          --  Fatal error
          cpu_rst <= (others => '1');
          reset_cause_cpu <= not cpu_rst_err;
          reset_cause_iram_ecc <= iram_ecc_fatal;
          reset_cause_dram_ecc <= dram_ecc_fatal;
          reset_cause_wd <= wd_timeout;
          reset_cause_recovery <= cpu_recovery_wr;
          cpu_flt_o <= cpu_flt;
          cpu_rst_o <= cpu_recovery_wr;
          state <= S_VOTER;
        end if;
      end if;
    end if;
  end process;  

  reset_cause_o(0) <= reset_cause_cpu;
  reset_cause_o(1) <= reset_cause_iram_ecc;
  reset_cause_o(2) <= reset_cause_dram_ecc;
  reset_cause_o(3) <= reset_cause_wd;
  reset_cause_o(4) <= reset_cause_recovery;
  reset_cause_o(31 downto 5) <= (others => '0');

  nbr_iram_ecc_corr_o <= nbr_iram_ecc_corr;
  nbr_iram_scrub_corr_o <= nbr_iram_scrub_corr;
  nbr_iram_scrub_uncorr_last_o <= nbr_iram_scrub_uncorr_last;
  nbr_iram_scrub_cycle_o <= nbr_iram_scrub_cycle;
  iram_scrub_en_o <= iram_scrub_en;

  nbr_dram_ecc_corr_o <= nbr_dram_ecc_corr;
  nbr_dram_scrub_corr_o <= nbr_dram_scrub_corr;
  nbr_dram_scrub_uncorr_last_o <= nbr_dram_scrub_uncorr_last;
  nbr_dram_scrub_cycle_o <= nbr_dram_scrub_cycle;
  dram_scrub_en_o <= dram_scrub_en;

end arch;
