library ieee;
use ieee.std_logic_1164.all;
use ieee.std_logic_arith.all;
use ieee.std_logic_unsigned.all;

library unisim;
use unisim.vcomponents.all;

--------------------------------------------------------------------------------

entity msp_motion is
  port (
    -- Clock & reset
    CLK_24MHz : in  std_logic;
    RESET     : in  std_logic;
    -- RS232
    RXD       : in  std_logic;
    TXD       : out std_logic;
    -- LED
    LED0      : out std_logic;
    LED1      : out std_logic;
    LED2      : out std_logic;
    -- PWM
    PWM0      : out std_logic;
    PWM0_EN   : out std_logic;
    PWM1      : out std_logic;
    PWM1_EN   : out std_logic;
    PWM2      : out std_logic;
    PWM2_EN   : out std_logic;
    -- IRC
    IRC_INDEX : in  std_logic;
    IRC_A     : in  std_logic;
    IRC_B     : in  std_logic;
    -- HALL
    HAL0      : in  std_logic;
    HAL1      : in  std_logic;
    HAL2      : in  std_logic);
end msp_motion;

--------------------------------------------------------------------------------

architecture rtl of msp_motion is

  signal reset_p : std_logic;

  ------------------------------------------------------------------------------
  -- OpenMSP430 softcore MCU module
  ------------------------------------------------------------------------------
  signal mclk      : std_logic;
  signal puc       : std_logic;
  -- External data bus
  signal dmem_addr : std_logic_vector (11 downto 0);
  signal dmem_ce   : std_logic;
  signal dmem_we   : std_logic;
  signal dmem_din  : std_logic_vector (15 downto 0);
  signal dmem_dout : std_logic_vector (15 downto 0);
  -- Peripheral bus
  signal per_din   : std_logic_vector (15 downto 0);
  signal per_dout  : std_logic_Vector (15 downto 0);
  signal per_wen   : std_logic_vector (1 downto 0);
  signal per_wen16 : std_logic;
  signal per_en    : std_logic;
  signal per_addr  : std_logic_vector (7 downto 0);
  -- Interrupt
  --signal irq       : std_logic_vector (13 downto 0) := (others => '0');
  signal irq       : std_logic_vector (13 downto 0);
  signal irq_acc   : std_logic_vector (13 downto 0);

  ------------------------------------------------------------------------------
  -- MCU peripherals
  ------------------------------------------------------------------------------
  -- GPIO
  signal GPIO_IN    : std_logic_vector (15 downto 0);
  signal GPIO_OUT   : std_logic_vector (15 downto 0);
  signal GPIO_DAT_O : std_logic_vector (15 downto 0);
  signal GPIO_SEL   : std_logic;
  -- Qcounter MCU interface
  signal QCNT_DAT_O : std_logic_vector (15 downto 0);
  signal QCNT_SEL   : std_logic;
  -- Motor feedback IRQ generator
  signal MOTOR_IRQ  : std_logic;
  -- Event register
  signal EVENT_DAT_O : std_logic_vector (15 downto 0);
  signal EVENT_SEL   : std_logic;
  signal event_i	 : std_logic_vector (15 downto 0);
  signal event_o	 : std_logic_vector (15 downto 0);
  -- Qcounter capture
  signal CAPTURE_SEL : std_logic;
  signal CAPTURE_DAT : std_logic_vector (15 downto 0);
  -- IRC index detection
  signal INDEX_DETECT_DAT_O : std_logic_vector (15 downto 0);  
  signal INDEX_DETECT_SEL : std_logic;

  ------------------------------------------------------------------------------
  -- Dual-port shared memory
  ------------------------------------------------------------------------------
  -- These signals of A-port (MCU) enables creation of external data encoder and
  -- multiplexer in a case of multiple devices connected to the external data
  -- bus. Otherwise useless.
  signal DPA_DAT_O   : std_logic_vector (15 downto 0);
  signal DPA_SEL     : std_logic;
  signal DPA_STB     : std_logic;
  -- Auxiliary register used to generate IRF_ACK
  signal IRF_ACK_REG : std_logic := '0';
  -- Auxiliary signal used to form B-port address
  signal DPB_ADR     : std_logic_vector (9 downto 0);

  ------------------------------------------------------------------------------
  -- Motion Control Chain
  ------------------------------------------------------------------------------
  -- Constants
  constant PWM_W     : integer := 10;
  constant LUT_ADR_W : integer := 11;
  constant LUT_INIT  : string  := "sin1000.lut";
  
  -- Bus interface to the shared memory
  signal IRF_ACK   : std_logic;
  signal IRF_ADR   : std_logic_vector (4 downto 0);
  signal IRF_DAT_I : std_logic_vector (15 downto 0);
  signal IRF_DAT_O : std_logic_vector (15 downto 0);
  signal IRF_STB   : std_logic;
  signal IRF_WE    : std_logic;
  -- Wave look-up table
  signal LUT_ADR   : std_logic_vector (LUT_ADR_W-1 downto 0);
  signal LUT_DAT_O : std_logic_vector (PWM_W-1 downto 0);
  signal LUT_STB   : std_logic;
  -- MCC execution control
  signal MCC_ACK   : std_logic;
  signal MCC_STB   : std_logic;

  ------------------------------------------------------------------------------
  -- PWM and IRC
  ------------------------------------------------------------------------------
  signal PWM_CNT  : std_logic_vector (PWM_W-1 downto 0);
  signal PWM_OW   : std_logic;          -- PWM counter overflow
  -- PWM interface to the MCC
  signal PWM_DAT  : std_logic_vector (PWM_W-1 downto 0);
  signal PWM1_STB : std_logic;
  signal PWM2_STB : std_logic;
  signal PWM3_STB : std_logic;
  -- PWM outputs
  signal PWM1_OUT : std_logic;
  signal PWM2_OUT : std_logic;
  signal PWM3_OUT : std_logic;
  -- IRC value
  signal QCNT     : std_logic_vector (31 downto 0);
  -- IRC capturing
  signal IRC_INDEX_DFF   : std_logic;   -- IRC_INDEX aligned with global clock
  signal IRC_INDEX_EVENT : std_logic;
  
--------------------------------------------------------------------------------

begin
  
  ------------------------------------------------------------------------------
  -- OpenMSP430 softcore MCU module
  ------------------------------------------------------------------------------
  openMSP430_1 : entity work.openMSP430_8_32_mul_dbus
    port map (
      dco_clk                  => CLK_24MHz,
      lfxt_clk                 => '0',
      reset_n                  => '1',
      rxd                      => RXD,
      txd                      => TXD,
      per_addr                 => per_addr,
      per_din                  => per_din,
      per_dout                 => per_dout,
      per_wen                  => per_wen,
      per_en                   => per_en,
      nmi                      => '0',
      irq                      => irq,
      irq_acc                  => irq_acc,
      aclk_en                  => open,
      smclk_en                 => open,
      mclk                     => mclk,
      puc                      => open,
      dmem_addr                => dmem_addr,
      dmem_ce                  => dmem_ce,
      dmem_we                  => dmem_we,
      dmem_din                 => dmem_din,
      dmem_dout                => dmem_dout);

  puc     <= '0';
  reset_p <= not RESET;

  STARTUP_VIRTEX2_inst : STARTUP_VIRTEX2
    port map (
      CLK => open,
      -- Clock input for start-up sequence
      GSR => reset_p, -- Global Set/Reset input (GSR cannot be used for the port name)
      GTS => open); -- Global 3-state input (GTS cannot be used for the port name)


  -- External data bus address decoder and data multiplexer.
  ------------------------------------------------------------------------------
  -- When connection more memories, be aware that 'dmem_dout' can vary only when
  -- reading cycle is performed. I.e. mux variable must be registered.
  dmem_dout <= DPA_DAT_O;
  
  DPA_SEL <= '1' when dmem_addr (11 downto 10) = "00" else '0';
  DPA_STB <= dmem_ce and DPA_SEL;

  -- Peripheral bus address decoder and data multiplexer.
  ------------------------------------------------------------------------------
  per_dout <= GPIO_DAT_O when GPIO_SEL = '1' else
              QCNT_DAT_O when QCNT_SEL = '1' else
              EVENT_DAT_O when EVENT_SEL = '1' else
              CAPTURE_DAT when CAPTURE_SEL = '1' else
			  INDEX_DETECT_DAT_O when INDEX_DETECT_SEL ='1' else
              (others => '0');          -- MUST be 0 when nothing is addressed
  
  GPIO_SEL <= '1' when per_addr(7 downto 2) = 16#0140#/2/4 else '0';
  QCNT_SEL <= '1' when per_addr(7 downto 1) = 16#0148#/2/2 else '0';
  EVENT_SEL <= '1' when per_addr(7 downto 0) = 16#014C#/2  else '0';
  CAPTURE_SEL <= '1' when per_addr(7 downto 1) = 16#0150#/2/2 else '0';
  INDEX_DETECT_SEL <= '1' when per_addr(7 downto 1) = 16#0152#/2/2 else '0';

  per_wen16 <= per_wen(0) and per_wen(1);

  -- Interrupt signals
  ------------------------------------------------------------------------------
  irq (13 downto 1) <= (others => '0');

  motor_irq_gen : process (mclk, puc) is
  begin
    if rising_edge (mclk) then
      if puc = '1' or irq_acc (0) = '1' then
        irq (0) <= '0';
      elsif MOTOR_IRQ = '1' then
        irq (0) <= '1';
      end if;
    end if;
  end process;

  
  ------------------------------------------------------------------------------
  -- MCU peripherals
  ------------------------------------------------------------------------------
  GPIO_IN(0) <= HAL0;
  GPIO_IN(1) <= HAL1;
  GPIO_IN(2) <= HAL2;
  GPIO_IN(3) <= IRC_INDEX;

  gpio_0 : entity work.gpio
    generic map (
      W => 16)
    port map (
      ACK_O  => open,
      ADR_I  => per_addr (1 downto 0),
      CLK_I  => mclk,
      DAT_I  => per_din,
      DAT_O  => GPIO_DAT_O,
      RST_I  => puc,
      SEL_I  => GPIO_SEL,
      STB_I  => per_en,
      WE_I   => per_wen16,
      GPIO_I => GPIO_IN,
      GPIO_O => GPIO_OUT);

  qcounter_mcu16_0 : entity work.qcounter_mcu16
    port map (
      ACK_O  => open,
      ADR_I  => per_addr (0),
      CLK_I  => mclk,
      DAT_O  => QCNT_DAT_O,
      SEL_I  => QCNT_SEL,
      STB_I  => per_en,
      QCOUNT => QCNT);

  -- Motor feedback IRQ generator
  -- f_motor_irq approx. 1 kHz
  irq_counter_1 : entity work.counter
    generic map (
      WIDTH => 5,
      MAX   => 22)
    port map (
      clk      => mclk,
      clk_en   => PWM_OW,
      reset    => puc,
      count    => open,
      event_ow => MOTOR_IRQ);

  event_io_0 : entity work.event_rwc
	port map (
       -- Peripheral bus interface
    ACK_O  => open,
    CLK_I  => mclk,
    DAT_I  => per_din,
    DAT_O  => EVENT_DAT_O,
    RST_I  => puc,
    SEL_I  => EVENT_SEL,
    STB_I  => per_en,
    WE_I   => per_wen16,
    -- Event port pins
    EVENT_I => event_i,
    EVENT_O => event_o);

  event_i(0)      <= (not IRC_INDEX_DFF) and (not event_o(0));
  IRC_INDEX_EVENT <= event_i(0);


  irc_index_dff_0 : entity work.dff
    port map (
      clock => mclk,
      d     => IRC_INDEX,
      q     => IRC_INDEX_DFF);

  capture_reg16_0 : entity work.capture_reg16
    port map (
      ACK_O     => open,
      ADR_I     => per_addr (0 downto 0),
      CLK_I     => mclk,
      DAT_O     => CAPTURE_DAT,
      SEL_I     => CAPTURE_SEL,
      STB_I     => per_en,
      EVENT_I   => IRC_INDEX_EVENT,
      CAPTURE_I => QCNT);

  ------------------------------------------------------------------------------
  -- Dual-port shared memory
  ------------------------------------------------------------------------------
  -- Shared memory between MCU and MCC (size: 16+2 bits x 1k).
  -- Port A (MCU side) has a priority of writing.
  shared_mem : RAMB16_S18_S18
    generic map (
      WRITE_MODE_A => "READ_FIRST",
      WRITE_MODE_B => "WRITE_FIRST")
    port map (
      -- A-Port (MCU)
      ADDRA => dmem_addr (9 downto 0),
      CLKA  => mclk,
      DIA   => dmem_din,
      DIPA  => "00",
      DOA   => DPA_DAT_O,
      DOPA  => open,
      ENA   => DPA_STB,
      SSRA  => '0',
      WEA   => dmem_we,
      -- B-Port (MCC)
      ADDRB => DPB_ADR,
      CLKB  => mclk,
      DIB   => IRF_DAT_I,
      DIPB  => "00",
      DOB   => IRF_DAT_O,
      DOPB  => open,
      ENB   => IRF_STB,
      SSRB  => '0',
      WEB   => IRF_WE);

  -- B-Port address (10 bits) constructed from IRF_ADR (5 bits). Upper addr bits
  -- are forced to '0', but in a case of several axes these can be used to
  -- address memory space of the appropriate one.
  DPB_ADR (9 downto 5) <= (others => '0');
  DPB_ADR (4 downto 0) <= IRF_ADR;
  
  -- Generation of IRF acknowledge signal for MCC.
  IRF_ACK <= IRF_STB and (IRF_WE or IRF_ACK_REG);

  -- IRF_ACK_REG signalizes that data is present on IRF_DAT_O when reading.
  irf_read : process (mclk, puc) is
  begin
    if rising_edge(mclk) then
      if puc = '1' then
        IRF_ACK_REG <= '0';
      else
        IRF_ACK_REG <= IRF_STB  and not IRF_WE;
      end if;
    end if;
  end process;

  
  ------------------------------------------------------------------------------
  -- Motion Control Chain
  ------------------------------------------------------------------------------
  mcc_exec_1 : entity work.mcc_exec
    generic map (
      AXIS_CNT   => 1,
      AXIS_CNT_W => 1)
    port map (
      CLK_I      => mclk,
      RST_I      => puc,
      MCC_AXIS_O => open,
      MCC_DONE_O => open,
      MCC_EN_I   => '1',
      MCC_EXEC_I => PWM_OW,
      MCC_ERR_O  => open,
      MCC_ACK_I  => MCC_ACK,
      MCC_STB_O  => MCC_STB);
  
  mcc_1 : entity work.mcc
    generic map (
      LUT_DAT_W => PWM_W,
      LUT_ADR_W => LUT_ADR_W)
    port map (
      ACK_O      => MCC_ACK,
      CLK_I      => mclk,
      RST_I      => puc,
      STB_I      => MCC_STB,
      LUT_STB_O  => LUT_STB,
      LUT_ADR_O  => LUT_ADR,
      LUT_DAT_I  => LUT_DAT_O,
      IRC_DAT_I  => QCNT (15 downto 0),
      PWM_DAT_O  => PWM_DAT,
      PWM1_STB_O => PWM1_STB,
      PWM2_STB_O => PWM2_STB,
      PWM3_STB_O => PWM3_STB,
      IRF_ACK_I  => IRF_ACK,
      IRF_ADR_O  => IRF_ADR,
      IRF_DAT_I  => IRF_DAT_O,
      IRF_DAT_O  => IRF_DAT_I,
      IRF_STB_O  => IRF_STB,
      IRF_WE_O   => IRF_WE);

  wave_table_1 : entity work.wave_table
    generic map (
      DAT_W     => PWM_W,
      ADR_W     => LUT_ADR_W,
      INIT_FILE => LUT_INIT)
    port map (
      ACK_O => open,
      ADR_I => LUT_ADR,
      CLK_I => mclk,
      DAT_I => conv_std_logic_vector(0, PWM_W),
      DAT_O => LUT_DAT_O,
      STB_I => LUT_STB,
      WE_I  => '0');


  ------------------------------------------------------------------------------
  -- PWM and IRC
  ------------------------------------------------------------------------------
  -- PWM counter is shared by all PWM generators. Generator contains only
  -- comparator and desired value.
  pwm_counter : entity work.counter
    generic map (
      WIDTH => PWM_W,
      MAX   => 2**PWM_W - 2)
    port map (
      clk      => mclk,
      clk_en   => '1',
      reset    => puc,
      count    => PWM_CNT,
      event_ow => PWM_OW);

    pwm_1 : entity work.pwm
    generic map (
      PWM_WIDTH => PWM_W)
    port map (
      clk     => mclk,
      reset   => puc,
      din     => PWM_DAT,
      sel     => '1',
      we      => PWM1_STB,
      pwm_cnt => PWM_CNT,
      pwm_cyc => PWM_OW,
      pwm     => PWM1_OUT);

  pwm_2 : entity work.pwm
    generic map (
      PWM_WIDTH => PWM_W)
    port map (
      clk     => mclk,
      reset   => puc,
      din     => PWM_DAT,
      sel     => '1',
      we      => PWM2_STB,
      pwm_cnt => PWM_CNT,
      pwm_cyc => PWM_OW,
      pwm     => PWM2_OUT);
  
  pwm_3 : entity work.pwm
    generic map (
      PWM_WIDTH => PWM_W)
    port map (
      clk     => mclk,
      reset   => puc,
      din     => PWM_DAT,
      sel     => '1',
      we      => PWM3_STB,
      pwm_cnt => PWM_CNT,
      pwm_cyc => PWM_OW,
      pwm     => PWM3_OUT);

  -- PWM signals mapped to FPGA outputs, EN forced to '1'
  PWM0    <= not PWM1_OUT;
  PWM0_EN <= '1';
  PWM1    <= not PWM2_OUT;
  PWM1_EN <= '1';
  PWM2    <= not PWM3_OUT;
  PWM2_EN <= '1';

  -- PWM is signalized on LEDs
  LED0    <= PWM1_OUT;
  LED1    <= PWM2_OUT;
  LED2    <= PWM3_OUT;

  qcounter_1 : entity work.qcounter
    port map (
      clock    => mclk,
      reset    => puc,
      a0       => IRC_A,
      b0       => IRC_B,
      qcount   => QCNT,
      a_rise   => open,
      a_fall   => open,
      b_rise   => open,
      b_fall   => open,
      ab_event => open,
      ab_error => open);

  
end rtl;