----------------------------------------------------------------------------------
-- Company: 
-- Engineer: fenrir
-- 
-- Create Date:     
-- Design Name: 
-- Module Name:    top - Behavioral 
-- Project Name: 
-- Target Devices: 
-- Tool versions: 
-- Description: 
--
----------------------------------------------------------------------------------

library IEEE;
use IEEE.STD_LOGIC_1164.ALL;
use IEEE.STD_LOGIC_ARITH.ALL;
use IEEE.STD_LOGIC_UNSIGNED.ALL;
library work;
use work.usefuls.all;

entity top is
  port (
      -- Global
      sysclk : in std_logic; -- 60MHz
      power_good : in std_logic;
      reset_n : in std_logic;
      ext_reset_n : out std_logic;
      led : out std_logic_vector(1 downto 0);
      debug : out std_logic_vector(3 downto 0);
      
      -- LEA-4T/6T
      gps_uart_rx : out std_logic;
      gps_uart_tx : in std_logic;
      gps_1pps : in std_logic;
      gps_1pps2 : in std_logic;
      gps_int0 : out std_logic;
      
      -- HMC5843
      mag_scl : inout std_logic;
      mag_sda : inout std_logic;
      
      -- ADS1248
      adc_start : out std_logic;
      adc_cs_n : out std_logic;
      adc_drdy_n : in std_logic;
      adc_dout : in std_logic;
      adc_din : out std_logic;
      adc_sclk : out std_logic;
      adc_reset_n : out std_logic;
      
      -- SPIROM
      spirom_sck : out std_logic;
      spirom_si : out std_logic;
      spirom_so : in std_logic;
      spirom_cs_n : out std_logic;
      
      -- Servo
      servo_in : in std_logic_vector(7 downto 0);
      servo_out : out std_logic_vector(7 downto 0);
      
      -- DSP (EMIFA)
      dsp_ema_d : inout std_logic_vector(7 downto 0);
      dsp_ema_a : in std_logic_vector(12 downto 0);
      dsp_ema_ba : in std_logic_vector(1 downto 0);
      dsp_ema_oe_n : in std_logic;
      dsp_ema_we_n : in std_logic;
      dsp_ema_wait: out std_logic;
      dsp_cs2_n : in std_logic;
      dsp_cs3_n : in std_logic;
      
      -- DSP(SPI0)
      dsp_spi0_simo_boot1 : inout std_logic;
      dsp_spi0_somi_boot0 : inout std_logic;
      dsp_spi0_clk_boot2 : inout std_logic;
      dsp_spi0_cs_n : in std_logic;
      
      -- DSP(I2C1)
      dsp_i2c1_sda_boot6 : inout std_logic;
      dsp_i2c1_scl_boot5 : inout std_logic;
      
      -- DSP(UART1)
      dsp_uart1_rxd : out std_logic;
      dsp_uart1_txd : in std_logic;
      
      -- DSP(UART2)
      dsp_uart2_rxd : out std_logic;
      dsp_uart2_txd : in std_logic;
      
      -- DSP(GPIO)
      dsp_gpio5_10 : out std_logic;
      dsp_gpio5_11 : out std_logic;
      
      -- DSP(Boot) @see Boot loader Appendix.A
      dsp_boot3 : inout std_logic;
      dsp_boot7 : inout std_logic;
      
      -- Extrenal uart (XBee etc.)
      ext_uart_tx : in std_logic;
      ext_uart_rx : out std_logic;
      
      -- External spi slave (ADS etc.)
      ext_spi_slave_clk : out std_logic;
      ext_spi_slave_simo : out std_logic;
      ext_spi_slave_somi : in std_logic;
      ext_spi_slave_cs_n : out std_logic_vector(2 downto 0);
      
      -- External auxiliary pins
      ext_aux : inout std_logic_vector(3 downto 0));
end top;

architecture Behavioral of top is
  
  signal reset : std_logic;
  signal clk_6M : std_logic;
  signal clk_1M, tc_1M : std_logic;
  signal clk_400, clk_200, clk_100, clk_50 : std_logic;
  signal clk_1 : std_logic;
  
  signal clk_400_3bits_counter : std_logic_vector(2 downto 0);
  
  signal clk_drive_servo : std_logic;
  
  component POR is
    generic (
        hold_count : natural := 1023);
    port (
        clk : in std_logic;
        reset_in : in std_logic;
        reset_out : out std_logic;
        reset_out_n : out std_logic);
  end component;
  
  component DividerN is
    generic (
        divide_value : positive := 8);
    port (
        clk, reset : in std_logic;
        clk_out, tc : out std_logic);
  end component;
  
  component Busmaster is
    generic (
        bus_width : integer := 8;
        address_width : integer := 8;
        spi_slave_devices : integer := 1);
    port (
        clk : in std_logic;
        
      -- external EMIFA
      emifa_data : inout std_logic_vector(bus_width-1 downto 0);
      emifa_address : in std_logic_vector(address_width-1 downto 0);
      emifa_oe_n : in std_logic;
      emifa_we_n : in std_logic;
      emifa_cs_n : in std_logic;
      emifa_wait : out std_logic;
      
      -- external SPI
      spi_cs_n : in std_logic;
      spi_clk : in std_logic;
      spi_mosi : in std_logic;
      spi_miso : out std_logic;
      
      spi_slave_cs_n : out std_logic_vector(spi_slave_devices-1 downto 0);
      spi_slave_miso : in std_logic_vector(spi_slave_devices-1 downto 0);
      
      -- internal bus IF
      internal_cs : out std_logic;
      internal_m2d : out std_logic_vector(bus_width-1 downto 0);
      internal_d2m : in std_logic_vector(bus_width-1 downto 0);
      internal_dir : out std_logic;    -- master to device => '1', device to master => '0'
      internal_addr : out std_logic_vector(address_width-1 downto 0);
      internal_done : in std_logic);  -- complete write / valid output
  end component;
  
  component ServoObserver is
    generic (
        bus_width : integer := 8;
        resolution : integer := 8;
        reset_value : integer := 0;
        threshold : integer := 0);
    port (
        clk : in std_logic;
        reset : in std_logic;
      
        -- original
        pulse_in : in std_logic;
        tick : in std_logic;
        updated : out std_logic;
        over_threshold : out std_logic;
      
        -- data IF
        data_in : in std_logic_vector(bus_width-1 downto 0);
        data_out : out std_logic_vector(bus_width-1 downto 0);
        dir : in std_logic;    -- read => '1', write => '0'
        cs : in std_logic;
        addr : in std_logic_vector(min_bits((resolution - 1) / bus_width) - 1 downto 0);
        done : out std_logic);  -- complete write / valid output
  end component;
  
  component ServoDriver is
    generic (
        bus_width : integer := 8;
        resolution : integer := 8;
        reset_value : integer := 0);
    port (
        clk : in std_logic;
        reset : in std_logic;
      
        -- original
        pulse_out : out std_logic;
        tick : in std_logic;
        drive : in std_logic;
      
        -- data IF
        data_in : in std_logic_vector(bus_width-1 downto 0);
        data_out : out std_logic_vector(bus_width-1 downto 0);
        dir : in std_logic;    -- read => '1', write => '0'
        cs : in std_logic;
        addr : in std_logic_vector(min_bits((resolution - 1) / bus_width) - 1 downto 0);
        done : out std_logic);  -- complete write / valid output
  end component;
  
  component i2c_mux is
    generic (
        slave_width : integer := 1;
        clk_ns : integer := 1000;  -- @ 1MHz
        minimum_scl_low_period_ns : integer := 1250;  -- @ 400KHz
        scl_stretch : boolean := true);
    port(
        scl : inout std_logic;
        sda : inout std_logic;
        clk : in std_logic;
        reset : in std_logic;
        scl_slave : inout std_logic_vector(slave_width - 1 downto 0);
        sda_slave : inout std_logic_vector(slave_width - 1 downto 0);
        slave_oe : in std_logic_vector(slave_width - 1 downto 0);
        transaction : out std_logic;      -- keep '1' in the same transaction
        slave2master : out std_logic);    -- M2S => '0', S2M => '1'
  end component;
  
  component ufm_pack
    port(
        ncs : in std_logic;
        sck : in std_logic;
        si : in std_logic;
        so : out std_logic);
  end component;
  
  constant internal_bus_width : integer := dsp_ema_d'length;
  constant dsp_ema_a_use : integer := 6;
  signal dsp_ema_addr : std_logic_vector(dsp_ema_a_use-1 downto 0);
  signal internal_cs : std_logic;
  signal internal_m2d : std_logic_vector(internal_bus_width-1 downto 0);
  signal internal_d2m : std_logic_vector(internal_bus_width-1 downto 0);
  signal internal_dir : std_logic;
  signal internal_addr : std_logic_vector(dsp_ema_a_use-1 downto 0);
  signal internal_done : std_logic;
  
  constant servo_resolution : integer := 12;
  
  type general_register_t is array (3 downto 0) of std_logic_vector(7 downto 0);
  signal general_register : general_register_t;
  signal function_register : std_logic_vector(7 downto 0);
  signal interrupt_flags : std_logic_vector(7 downto 0);
  signal function_register_2 : std_logic_vector(7 downto 0);
  
  signal servo_driver_cs : std_logic_vector(servo_out'range);
  signal servo_observer_cs : std_logic_vector(servo_in'range);
  
  constant addr_base_servo_obs : integer := 0;
  constant addr_base_servo_drv : integer := addr_base_servo_obs + (servo_in'length * 2);
  constant addr_base_general_register : integer := addr_base_servo_drv + (servo_out'length * 2);
  constant addr_base_unused : integer := addr_base_general_register + general_register'length;
  
  signal cic_watchdog : std_logic_vector(log2_ceil(32) downto 0);
  signal cic_updated : std_logic;
  signal cic_assert : std_logic;
  signal cic_servo_out : std_logic_vector(servo_out'range);
  signal cic_force_use : boolean;
  
  signal adc_cs_n_buf : std_logic;
  signal adc_drdy_n_buf : std_logic;
  
  signal buf_gps_1pps : std_logic;
  signal sys_1pps : std_logic;
  
  signal reset_gen_400 : std_logic;
  signal reset_gen_1 : std_logic;
  
  signal dsp_boot_locking : std_logic;
  
  signal i2c_mux_reset : std_logic;
  signal i2c_transaction : std_logic;
  signal i2c_int_en : std_logic;
  signal i2c_ext_en : std_logic;
  signal i2c_ext_exist : boolean;
  
  signal spi0_mux_cs_n : std_logic;
  signal spi0_mux_miso : std_logic;
  
  signal ext_spi_slave_somi_internal : std_logic_vector(1 downto 0);
  
  signal gps_uart_tx_selected : std_logic;
  signal gps_1pps_selected : std_logic;
  
  constant generate_ufm : boolean := true;
  signal ufm_cs_n : std_logic;
  signal ufm_sck : std_logic;
  signal ufm_si : std_logic;
  signal ufm_so : std_logic;
  
  type special_function is (none, ina111, gps_slave);
  constant generate_special : special_function := none;
begin

  -- debug
  debug(0) <= dsp_spi0_simo_boot1;
  debug(1) <= dsp_spi0_somi_boot0;
  debug(2) <= dsp_spi0_clk_boot2;
  debug(3) <= dsp_spi0_cs_n;
  
  reset <= not reset_n;
  adc_reset_n <= reset_n;

  div1 : DividerN
    generic map (
        divide_value => 10)    -- 60MHz => 6MHz
    port map (
        clk => sysclk,
        reset => reset,
        clk_out => clk_6M);

  div2 : DividerN
    generic map (
        divide_value => 6)     -- 6MHz => 1MHz
    port map (
        clk => clk_6M,
        reset => reset,
        clk_out => clk_1M,
        tc => tc_1M);
  
  div3 : DividerN
    generic map (
        divide_value => 2500) -- 1MHz => 400Hz
    port map (
        clk => clk_1M,
        reset => reset_gen_400,
        clk_out => clk_400);
        
  process(clk_400, reset_gen_400) begin
    if reset_gen_400 = '1' then
      clk_400_3bits_counter <= (others => '0');
    elsif clk_400'event and clk_400 = '1' then
      clk_400_3bits_counter <= clk_400_3bits_counter + 1;
    end if;
  end process;
  
  clk_200 <= clk_400_3bits_counter(0);
  clk_100 <= clk_400_3bits_counter(1);
  clk_50 <= clk_400_3bits_counter(2);
  
  div4 : DividerN
    generic map (
        divide_value => 50)   -- 50Hz => 1Hz
    port map (
        clk => clk_50,
        reset => reset_gen_1,
        clk_out => clk_1);
  
  led(0) <= clk_1;
  led(1) <= internal_cs or i2c_transaction;
  
  process(sysclk, reset) begin
    if reset = '1' then
      buf_gps_1pps <= '0';
    elsif sysclk'event and sysclk = '1' then
      buf_gps_1pps <= gps_1pps_selected;
    end if;
  end process;
  
  sys_1pps <= '1' when buf_gps_1pps = '1' and gps_1pps_selected = '0'
      else '0';
      
  dsp_uart1_rxd <= gps_uart_tx_selected;
  gps_uart_rx <= dsp_uart1_txd;
  
  dsp_ema_addr <= dsp_ema_a(dsp_ema_a_use - 3 downto 0) & dsp_ema_ba;
  
  -- i2c MUX
  i2c : i2c_mux
    generic map (
        slave_width => 2,
        clk_ns => 167,
        minimum_scl_low_period_ns => 1250, -- corresponding to 400KHz => 1s / 400K / 2 = 1250ns
        scl_stretch => true) 
    port map (
        scl => dsp_i2c1_scl_boot5,
        sda => dsp_i2c1_sda_boot6,
        clk => clk_6M,
        reset => i2c_mux_reset, 
        scl_slave(0) => mag_scl,
        sda_slave(0) => mag_sda,
        slave_oe(0) => i2c_int_en,
        scl_slave(1) => ext_aux(2),
        sda_slave(1) => ext_aux(3),
        slave_oe(1) => i2c_ext_en,
        transaction => i2c_transaction);
  
  busmater : Busmaster
    generic map (
        bus_width => dsp_ema_d'length,
        address_width => dsp_ema_a_use,
        spi_slave_devices => 4)
    port map (
        clk => sysclk,
      
        -- external EMIFA
        emifa_data => dsp_ema_d,
        emifa_address => dsp_ema_addr,
        emifa_oe_n => dsp_ema_oe_n,
        emifa_we_n => dsp_ema_we_n,
        emifa_cs_n => dsp_cs3_n,
        emifa_wait => dsp_ema_wait,
      
        -- external SPI
        spi_cs_n => spi0_mux_cs_n,
        spi_clk => dsp_spi0_clk_boot2,
        spi_mosi => dsp_spi0_simo_boot1,
        spi_miso => spi0_mux_miso,
        
        spi_slave_cs_n(0) => ext_spi_slave_cs_n(0),
        spi_slave_miso(0) => ext_spi_slave_somi_internal(0),
        spi_slave_cs_n(1) => ext_spi_slave_cs_n(1),
        spi_slave_miso(1) => ext_spi_slave_somi_internal(1),
        spi_slave_cs_n(2) => ext_spi_slave_cs_n(2),
        spi_slave_miso(2) => ext_spi_slave_somi,
        spi_slave_cs_n(3) => adc_cs_n_buf,
        spi_slave_miso(3) => adc_dout,
        
        -- internal bus
        internal_cs => internal_cs,
        internal_m2d => internal_m2d,
        internal_d2m => internal_d2m,
        internal_dir => internal_dir,
        internal_addr => internal_addr,
        internal_done => internal_done);
  
  -- Input captuer from R/C receiver
  gen_servo_obs : for i in servo_in'range generate
    gen_servo_obs_normal: if i < servo_in'high generate
      servo_obs_normal : ServoObserver
        generic map (
            bus_width => internal_bus_width,
            resolution => servo_resolution, 
            reset_value => 0)
        port map (
            clk => sysclk,
            reset => reset,
            pulse_in => servo_in(i),
            tick => clk_1M,
            --updated => 'Z',
            data_in => internal_m2d,
            data_out => internal_d2m,
            dir => internal_dir,
            cs => servo_observer_cs(i),
            addr => internal_addr(0 downto 0),
            done => internal_done);
    end generate;
    gen_servo_obs_pic_cic: if i = servo_in'high generate
      servo_obs_pic_cic : ServoObserver
        generic map (
            bus_width => internal_bus_width,
            resolution => servo_resolution, 
            reset_value => 0,
            threshold => 1500)
        port map (
            clk => sysclk,
            reset => reset,
            pulse_in => servo_in(i),
            tick => clk_1M,
            updated => cic_updated,
            over_threshold => cic_assert,
            data_in => internal_m2d,
            data_out => internal_d2m,
            dir => internal_dir,
            cs => servo_observer_cs(i),
            addr => internal_addr(0 downto 0),
            done => internal_done);
    end generate;
    
    servo_observer_cs(i) <= '1' when internal_cs = '1'
          and internal_addr(internal_addr'high downto 1) = conv_std_logic_vector(
              (addr_base_servo_obs / 2) + i, internal_addr'length)
        else '0';
  end generate;
  
  -- Output in CIC mode
  gen_servo_drv : for i in servo_out'range generate
    servo_drv : ServoDriver
      generic map (
          bus_width => internal_bus_width,
          resolution => servo_resolution, 
          reset_value => 0) -- free(0) or neutral(1500)
      port map (
          clk => sysclk,
          reset => reset,
          pulse_out => cic_servo_out(i),
          tick => clk_1M,
          drive => clk_drive_servo,
          data_in => internal_m2d,
          data_out => internal_d2m,
          dir => internal_dir,
          cs => servo_driver_cs(i),
          addr => internal_addr(0 downto 0),
          done => internal_done);
    
    servo_driver_cs(i) <= '1' when internal_cs = '1'
          and internal_addr(internal_addr'high downto 1) = conv_std_logic_vector(
              (addr_base_servo_drv / 2) + i, internal_addr'length)
        else '0';
  end generate;
  
  -- CIC/PIC changing function
  process(clk_50, cic_updated, cic_assert) begin
    if cic_updated = '1' then
      if cic_assert = '1' then
        cic_watchdog <= (others => '1');
      else
        cic_watchdog <= (others => '0');
      end if;
    elsif clk_50'event and clk_50 = '1' then
      if cic_watchdog > conv_std_logic_vector(0, cic_watchdog'length) then
        cic_watchdog <= cic_watchdog - 1;
      else
        cic_watchdog <= cic_watchdog;
      end if;
    end if;
  end process;
  
  servo_out <= cic_servo_out 
        when (cic_watchdog > conv_std_logic_vector(0, cic_watchdog'length))
          or cic_force_use
      else servo_in;
  
  -- UFM
  gen_ufm : if generate_ufm generate
    ufm : ufm_pack
      port map (
          ncs => ufm_cs_n,
          sck => ufm_sck,
          si => ufm_si,
          so => ufm_so);
  end generate;
  
  gen_none_ufm : if not generate_ufm generate
    ufm_so <= '1';
  end generate;
  
  -- general_register
  gen_register : for i in general_register'length - 1 downto 0 generate
    process(sysclk, reset)
      variable general_register_buf : std_logic_vector(general_register(i)'high downto 0); 
    begin
      if reset = '1' then
        general_register(i) <= (others => '0');
      elsif sysclk'event and sysclk = '1' then
        if internal_cs = '1' 
            and internal_addr = conv_std_logic_vector(addr_base_general_register + i, internal_addr'length)
            and internal_dir = '1' then
          general_register(i) <= internal_m2d;
        else
          general_register_buf := general_register(i);
          if i = 0 then -- function register
            if dsp_boot_locking = '0' then
              general_register_buf(0) := '1'; 
            end if;
          elsif i = 1 then -- interrupt flags
            if adc_drdy_n = '1' and adc_drdy_n_buf = '0' then
              general_register_buf(0) := '1'; 
            end if;
          end if;
          general_register(i) <= general_register_buf;
        end if;
      end if;
    end process;
    
    process(sysclk) begin
      if sysclk'event and sysclk = '1' then
        if internal_cs = '1'
            and internal_addr = conv_std_logic_vector(addr_base_general_register + i, internal_addr'length) then
          internal_done <= '1';
          internal_d2m <= general_register(i);
        else
          internal_done <= 'Z';
          internal_d2m <= (others => 'Z');
        end if;
      end if;
    end process;
  end generate;
  
  -- unused address
  process(sysclk) begin
    if sysclk'event and sysclk = '1' then
      if internal_cs = '1'
          and internal_addr >= conv_std_logic_vector(addr_base_unused, internal_addr'length) then
        internal_done <= '1';
        internal_d2m <= (others => '0');
      else
        internal_done <= 'Z';
        internal_d2m <= (others => 'Z');
      end if;
    end if;
  end process;
  
  -- done terminator
  process(sysclk) begin
    if sysclk'event and sysclk = '1' then
      if internal_cs = '0' then
        internal_done <= '0';
      else
        internal_done <= 'Z';
      end if;
    end if;
  end process;
  
  -- function register(general_register(0))
  function_register <= general_register(0);
  -- 0: boot lock (0: locked, 1: unlocked)
  -- SPI0 Flash boot => boot[7,2,1,0] => [0,1,0,1] 
  dsp_spi0_somi_boot0 <= '1' when function_register(0) = '0'
      else spirom_so when function_register(2 downto 1) = "00"
      else ufm_so when function_register(2 downto 1) = "01"
      else spi0_mux_miso when function_register(2) = '1'
      else 'Z';
  dsp_spi0_simo_boot1 <= '0' when function_register(0) = '0'
      else 'Z';
  dsp_spi0_clk_boot2 <= '1' when function_register(0) = '0'
      else 'Z';
  dsp_boot3 <= '0' when function_register(0) = '0'
      else 'Z';
  dsp_i2c1_scl_boot5 <= '0' when function_register(0) = '0'
      else 'Z';
  dsp_i2c1_sda_boot6 <= '0' when function_register(0) = '0'
      else 'Z';
  dsp_boot7 <= '0' when function_register(0) = '0'
      else 'Z';
  
  i2c_mux_reset <= '1' when (function_register(0) = '0')
      else '0';
  
  dsp_boot_hold : POR
    generic map (
        hold_count => 31)
    port map (
        clk => sysclk,
        reset_in => reset,
        reset_out => dsp_boot_locking);
      
  -- 2, 1: spi0 switch (00: SPIROM, 01: UFM, 1X: MUX device)
  spirom_cs_n <= dsp_spi0_cs_n when function_register(2 downto 1) = "00"
      else '1';
  spirom_sck <= dsp_spi0_clk_boot2 when function_register(0) /= '0'
      else '1';
  spirom_si <= dsp_spi0_simo_boot1 when function_register(0) /= '0'
      else '1';
  
  ufm_cs_n <= dsp_spi0_cs_n when function_register(2 downto 1) = "01"
      else '1';
  ufm_sck <= dsp_spi0_clk_boot2 when function_register(0) /= '0'
      else '0';
  ufm_si <= dsp_spi0_simo_boot1 when function_register(0) /= '0'
      else '1';
  
  spi0_mux_cs_n <= dsp_spi0_cs_n when function_register(2) = '1'
      else '1';
  adc_sclk <= not dsp_spi0_clk_boot2 when function_register(0) /= '0' and adc_cs_n_buf = '0'
      else '0';
  adc_din <= dsp_spi0_simo_boot1 when function_register(0) /= '0' and adc_cs_n_buf = '0'
      else '1';
  ext_spi_slave_clk <= dsp_spi0_clk_boot2 when function_register(0) /= '0'
      else '1';
  ext_spi_slave_simo <= dsp_spi0_simo_boot1 when function_register(0) /= '0'
      else '1';
  adc_cs_n <= adc_cs_n_buf;
  
  -- 4, 3: adc start control (0X: 0, 10: 1, 11: sync_to_clk_100)
  adc_start <= clk_100 when function_register(4 downto 3) = "11"
      else '1' when function_register(4 downto 3) = "10"
      else '0';
  
  -- 5: auto 400/200/100/50 Hz clock gen sync to 1pps (0: nop, 1: sync_to_1pps)
  reset_gen_400 <= (reset or sys_1pps) when function_register(5) = '1'
      else reset;
  
  -- 6: auto 1 Hz clock gen sync to 1pps (0: nop, 1: sync_to_1pps)
  reset_gen_1 <= (reset or sys_1pps) when function_register(6) = '1'
      else reset;
  
  -- 7: assert -ext_reset_n
  ext_reset_n <= not function_register(7);
  
  -- interrupt flags(general_register(1))
  interrupt_flags <= general_register(1);
  
  process(sysclk) begin
    if sysclk'event and sysclk = '1' then
      adc_drdy_n_buf <= adc_drdy_n;
    end if;
  end process;
  
  -- interrput
  dsp_gpio5_10 <= gps_1pps_selected;
  dsp_gpio5_11 <= '0' 
      when (interrupt_flags /= conv_std_logic_vector(0, interrupt_flags'length)
        and function_register(0) /= '0')
      else '1';
  
  -- function register2 (general_register(2))
  function_register_2 <= general_register(2);
  -- 1, 0: Servo drive ratio control (00: 50Hz, 01: 100Hz, 10: 200Hz, 11: 400Hz)
  clk_drive_servo <= clk_400 when function_register_2(1 downto 0) = "11"
      else clk_200 when function_register_2(1 downto 0) = "10"
      else clk_100 when function_register_2(1 downto 0) = "01"
      else clk_50;
  
  -- 2: Force to use CIC servo out
  cic_force_use <= true when function_register_2(2) = '1'
      else false;
      
  -- 3: Disable internal I2C components
  i2c_int_en <= '1' when function_register_2(3) /= '1'
      else '0';
  
  -- 4: Disable external I2C components
  i2c_ext_en <= '1' when function_register_2(4) /= '1' and i2c_ext_exist
      else '0';
  
  -- ext uart
  dsp_uart2_rxd <= ext_uart_tx;
  ext_uart_rx <= dsp_uart2_txd;
  
  ext_aux <= (others => 'Z');
  ext_spi_slave_somi_internal <= (others => 'Z');
  
  -- normal
  gen_normal: if generate_special = none generate
    ext_aux(1 downto 0) <= gps_1pps & gps_uart_tx;
    gps_1pps_selected <= gps_1pps;
    gps_uart_tx_selected <= gps_uart_tx;
    i2c_ext_exist <= true;
  end generate;
  
  -- @ina111 rocket module
  gen_ina111: if generate_special = ina111 generate
    ext_aux(3) <= not dsp_spi0_clk_boot2 when function_register(0) /= '0'
        else '0';
    ext_aux(2) <= dsp_spi0_simo_boot1 when function_register(0) /= '0'
        else '1';
    ext_spi_slave_somi_internal(1 downto 0) <= ext_aux(1) & ext_aux(1);
    gps_1pps_selected <= gps_1pps;
    gps_uart_tx_selected <= gps_uart_tx;
    i2c_ext_exist <= false;
  end generate;
  
  -- gps slave
  gen_gps_slave: if generate_special = gps_slave generate
    gps_1pps_selected <= ext_aux(1);
    gps_uart_tx_selected <= ext_aux(0);
    i2c_ext_exist <= true;
  end generate;
end Behavioral;