------------------------------------------------------------------------------
--  Copyright (C) 2011 -  Kenichi Kurimoto
--
--  This program is free software; you can redistribute it and/or modify
--  it under the terms of the GNU General Public License as published by
--  the Free Software Foundation; either version 2 of the License, or
--  (at your option) any later version.
--
--  This program is distributed in the hope that it will be useful,
--  but WITHOUT ANY WARRANTY; without even the implied warranty of
--  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
--  GNU General Public License for more details.
--
--  You should have received a copy of the GNU General Public License
--  along with this program; if not, write to the Free Software
--  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA 
-----------------------------------------------------------------------------
-- Entity:      yccrgbs
-- File:        yccrgbs.vhd
-- Author:      Kenichi Kurimoto 
-- Description: simple YCbCr-RGB translation 
------------------------------------------------------------------------------

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

library grlib;
use grlib.amba.all;
use grlib.stdlib.all;
use grlib.devices.all;

library techmap;
use techmap.gencomp.all;

entity yccrgbs is
   generic (
      memtech : integer := DEFMEMTECH;
      fifo_depth : integer := 32;
      burst_num : integer := 16;
      shindex : integer := 0;
      haddr  : integer := 0;
      hmask  : integer := 16#fff#;
      hirq   : integer := 0;      
      pindex : integer := 0;
      paddr  : integer := 0;
      pmask  : integer := 16#fff#;
      mhindex : integer := 0;
      chprot : integer := 3);      

   port (
      rst   : in std_ulogic;
      clk   : in std_ulogic;
      ahbmi : in ahb_mst_in_type;
      ahbmo : out ahb_mst_out_type;
      ahbsi : in ahb_slv_in_type;
      ahbso : out ahb_slv_out_type;
      apbi  : in apb_slv_in_type;
      apbo  : out apb_slv_out_type
   );
end;

architecture rtl of yccrgbs is

constant shconfig : ahb_config_type := (
 0 => ahb_device_reg( VENDOR_CONTRIB, CONTRIB_CORE1, 0, 0, hirq),
 4 => ahb_membar(haddr, '0', '0', hmask),
 others => zero32);
 
constant mhconfig : ahb_config_type := (
 0 => ahb_device_reg( VENDOR_CONTRIB, CONTRIB_CORE1, 0, 0, 0),
 others => zero32);
  
constant pconfig : apb_config_type := (
 0 => ahb_device_reg( VENDOR_CONTRIB, CONTRIB_CORE1, 0, 0, 0),
 1 => apb_iobar(paddr, pmask));
 
function stdv2str(vec:std_logic_vector) return string is
   variable str : string(vec'length downto 1);
   begin
   for i in vec'length-1 downto 0 loop
      if(vec(i)='U') then
          str(i+1) := 'U';
      elsif(vec(i)='X') then
          str(i+1) := 'X';
      elsif(vec(i)='0') then
          str(i+1) := '0';
      elsif(vec(i)='1') then
           str(i+1) := '1';
      elsif(vec(i)='Z') then
           str(i+1) := 'Z';
      elsif(vec(i)='W') then
           str(i+1) := 'W';
      elsif(vec(i)='L') then
           str(i+1) := 'L';
      elsif(vec(i)='H') then
           str(i+1) := 'H';
      else
           str(i+1) := '-';
      end if;
   end loop;
   return str;
end;   
          
function mysigned_mul(a,b : std_logic_vector) return std_logic_vector is
variable z : std_logic_vector(a'length + b'length -1 downto 0);
begin
    z := std_logic_vector(signed(a) * signed(b));
    return(z);
end;


function mysigned_add(a,b : std_logic_vector) return std_logic_vector is
variable ex_a : std_logic_vector(a'length downto 0);
variable ex_b : std_logic_vector(b'length downto 0);
variable z1 : std_logic_vector(a'length downto 0);
variable z2 : std_logic_vector(b'length downto 0);
begin
    ex_a := a(a'left) & a;
    ex_b := b(b'left) & b;
    if( a'length > b'length)then
       z1 := std_logic_vector(signed(ex_a) + signed(ex_b));
       return(z1);
    else
       z2 := std_logic_vector(signed(ex_a) + signed(ex_b));
       return(z2);
    end if;
end;

procedure yc_datapath(
    yin     : in  std_logic_vector(7 downto 0);
    cbin    : in  std_logic_vector(7 downto 0);
    crin    : in  std_logic_vector(7 downto 0);
    rout    : out std_logic_vector(4 downto 0);
    gout    : out std_logic_vector(5 downto 0);
    bout    : out std_logic_vector(4 downto 0)) is
   variable node1 : std_logic_vector(9 downto 0);
   variable node2 : std_logic_vector(9 downto 0);
   variable node3 : std_logic_vector(15 downto 0);
   variable node4 : std_logic_vector(15 downto 0);
   variable node5 : std_logic_vector(15 downto 0);
   variable node6 : std_logic_vector(15 downto 0);
   variable node7 : std_logic_vector(9 downto 0);
   variable node8 : std_logic_vector(8 downto 0);
   variable node9 : std_logic_vector(9 downto 0);
   variable node10 : std_logic_vector(9 downto 0);
begin
      node1 := mysigned_add(('0' & cbin), "110000000");
      node2 := mysigned_add(('0' & crin), "110000000");
      node3 := mysigned_mul("01011010", node2(7 downto 0));
      node4 := mysigned_mul("11101010", node1(7 downto 0));
      node5 := mysigned_mul("11010010", node2(7 downto 0));
      node6 := mysigned_mul("01110001", node1(7 downto 0));
      node7 := mysigned_add(('0' & yin), (node3(14 downto 7) & '0'));
      node8 := mysigned_add((node4(13 downto 7) & '0'), (node5(13 downto 7) & '0'));
      node9 := mysigned_add(('0' & yin), ( node6(14 downto 7) & '0'));
      node10 := mysigned_add(('0' & yin), node8);
      if(node7(9) = '1') then
         rout := "00000";
      elsif(node7(8) = '1') then
         rout := "11111";
      else
         rout := node7(7 downto 3);
      end if;
     
      if(node10(9) = '1') then
         gout := "000000";
      elsif(node10(8) = '1') then
         gout := "111111";
      else
         gout := node10(7 downto 2);
      end if;
      
      if(node9(9) = '1') then
         bout := "00000";
      elsif(node9(8) = '1') then
         bout := "11111";
      else
         bout := node9(7 downto 3);
      end if;           
end;

type fstate_type is (ready, stuck);
type mstate_type is (idle, busreq, grant, nonseq, seq);

type ahbs_reg is record
   get : std_logic;
   ycc    : std_logic_vector(23 downto 0);
   written : std_ulogic;
end record;

type apbs_reg is record
   fb_add    : std_logic_vector(31 downto 0);
   size_info : std_logic_vector(31 downto 0);
   inc_add   : std_logic_vector(11 downto 0);
end record;

type control_reg is record
   fifo_state : fstate_type;
   fifo_rp : std_logic_vector(7 downto 0);
   fifo_wp : std_logic_vector(7 downto 0);
   gen_add : std_logic_vector(31 downto 0);
   temp_data : std_logic_vector(15 downto 0);
   mstate : mstate_type;
   xpoint : std_logic_vector(15 downto 0);
   ypoint : std_logic_vector(15 downto 0);
   bst_cnt : integer;
   keep_data : std_logic_vector(31 downto 0);
   dhready : std_logic;
end record;

type all_reg is record
   hreg : ahbs_reg;
   preg : apbs_reg;
   creg : control_reg;
end record;



signal r, rin : all_reg;
signal read_en_fifo, write_en_fifo : std_logic;
signal read_pointer_fifo : std_logic_vector(5 downto 0);
signal write_pointer_fifo : std_logic_vector(5 downto 0);
signal data_out_fifo : std_logic_vector(31 downto 0);
signal data_in_fifo : std_logic_vector(31 downto 0);

begin
ram0 : syncram_2p generic map(tech => memtech, abits => 6, dbits => 32,
            sepclk => 0)
            port map( clk, read_en_fifo, read_pointer_fifo, data_out_fifo,
                clk, write_en_fifo, write_pointer_fifo, data_in_fifo);

comb : process (r, rst, ahbsi, ahbmi, apbi, data_out_fifo)
      variable v : all_reg;
      variable apbwrite : std_logic;
      variable vprdata : std_logic_vector(31 downto 0);
      variable virq : std_logic_vector(NAHBIRQ-1 downto 0);
      variable vsready : std_logic;
      variable red : std_logic_vector(4 downto 0);
      variable green : std_logic_vector(5 downto 0);
      variable blue : std_logic_vector(4 downto 0);
      variable fifo_write : std_logic;
      variable fifo_read :std_logic;
      variable write_point : integer;
      variable read_point : integer;
      variable num_ele : integer;
      variable mhaddr :  std_logic_vector(31 downto 0);
      variable mhtrans : std_logic_vector(1 downto 0);
      variable mhbusreq : std_ulogic;
      variable mhwdata : std_logic_vector(31 downto 0);
      variable mhprot : std_logic_vector(3 downto 0);
      variable add_inc : std_logic;
      variable x : std_logic_vector(15 downto 0);
      variable y : std_logic_vector(15 downto 0);
      variable maxx,maxy : std_logic_vector(15 downto 0);
      variable vstart : std_logic;
      variable vmhwdata : std_logic_vector(31 downto 0);
      
      
    begin

   v := r;
   vprdata := (others => '0'); 
   virq := (others => '0');
   mhprot := conv_std_logic_vector(chprot,4);
   v.hreg.written := '0';
   fifo_write := '0';
   vsready := '1';
   vstart := '0';
   mhbusreq := '0';
   mhtrans := HTRANS_IDLE;
   
-- apb controle part
   apbwrite := apbi.psel(pindex) and apbi.pwrite and apbi.penable;
   vprdata := (others => '0');
   case apbi.paddr(5 downto 2) is
   when "0000" =>
      if apbwrite = '1' then
         v.preg.fb_add := apbi.pwdata(31 downto 0);
      end if;
      vprdata := v.preg.fb_add(31 downto 0);
   when "0001" =>
      if apbwrite = '1' then
         v.preg.size_info := apbi.pwdata(31 downto 0);
      end if;
      vprdata := v.preg.size_info(31 downto 0);
   when "0010" =>
      if apbwrite = '1' then
         v.preg.inc_add := apbi.pwdata(11 downto 0);
      end if;
      vprdata := "00000000000000000000" & v.preg.inc_add(11 downto 0);
   when "0011" =>
      if apbwrite = '1' then
          if apbi.pwdata(0) = '1' then
              vstart := '1';
          end if;        
      end if;
      vprdata := (others => '0');
   when others =>
   end case;  
   

   v.hreg.get := '0';
-- ahb slave input part
   if (ahbsi.hready = '1') and (ahbsi.hsel(shindex) = '1') then
       if (ahbsi.haddr(15 downto 0) = "0000000000000000") and (ahbsi.hwrite = '1') and (ahbsi.htrans = HTRANS_NONSEQ) then
           v.hreg.get := '1';
       end if;
   end if;
   if r.hreg.get = '1' then
        v.hreg.ycc(23 downto 0) := ahbsi.hwdata(23 downto 0);
        v.hreg.written := '1';            
   end if;
   
   
-- register write   
   yc_datapath(r.hreg.ycc(23 downto 16), r.hreg.ycc(15 downto 8),r.hreg.ycc(7 downto 0), red,green,blue);

-- Fifo part
   write_point := 0;
   if notx(r.creg.fifo_wp) then write_point := to_integer(unsigned(r.creg.fifo_wp)); end if;
   read_point := 0;
   if notx(r.creg.fifo_rp) then read_point := to_integer(unsigned(r.creg.fifo_rp)); end if;
   if write_point >= read_point then
       num_ele := write_point - read_point;
   else
       num_ele := fifo_depth - read_point + write_point;
   end if;
   if num_ele > fifo_depth/2 then
       vsready := '0';
   end if;
   if (num_ele = fifo_depth/2) and ( r.hreg.written = '1' or r.creg.fifo_state = stuck) then
       vsready := '0';
   end if;
   
   case r.creg.fifo_state is
   when ready =>
      if r.hreg.written = '1' then
         v.creg.temp_data := red & green & blue;
         v.creg.fifo_state := stuck;
      end if;
   when stuck =>
      if r.hreg.written = '1' then
         fifo_write := '1';
         write_point := write_point + 1;
         if write_point = fifo_depth then
            write_point := 0;
         end if;
         v.creg.fifo_state := ready;
      end if;
   end case; 
      
--   data_in_fifo <= red & green & blue & r.creg.temp_data; 
   data_in_fifo <= r.creg.temp_data &red & green & blue; 
   write_en_fifo <= fifo_write;
   v.creg.fifo_wp := std_logic_vector(to_unsigned(write_point,8));

-- AHB master part
   fifo_read := '0';
   add_inc := '0';
   maxx := "0" & r.preg.size_info(31 downto 17);
   maxy := r.preg.size_info(15 downto 0);
   
   case r.creg.mstate is
   when idle =>
       mhbusreq := '0';
       mhtrans := HTRANS_IDLE;
       v.creg.bst_cnt := burst_num;
       if (num_ele >= burst_num) then
           v.creg.mstate := busreq;
       end if;
   when busreq =>
       mhbusreq := '1';
       mhtrans := HTRANS_NONSEQ;
       if  (ahbmi.hready = '1') and (ahbmi.hgrant(mhindex) = '1') then
           v.creg.mstate := nonseq;
       end if;
   when nonseq =>
       mhbusreq := '1';
       mhtrans := HTRANS_NONSEQ;
       if ahbmi.hready = '1' then
           fifo_read := '1';
           add_inc := '1';
           if r.creg.xpoint = maxx -1 then
              v.creg.mstate := idle;
           else
              v.creg.mstate := seq;           
              v.creg.bst_cnt := v.creg.bst_cnt - 1;
           end if;
       end if;
   when seq =>
       mhbusreq := '1';
       mhtrans := HTRANS_SEQ;
       if ahbmi.hready = '1' then
           fifo_read := '1';
           add_inc := '1';
           v.creg.bst_cnt := v.creg.bst_cnt - 1;
           if (v.creg.bst_cnt = 0) or (r.creg.xpoint = maxx -1) then
               v.creg.mstate := idle;
           end if;
       end if;
   when others =>
   end case;
   
   mhaddr := r.creg.gen_add;
   x := r.creg.xpoint;
   y := r.creg.ypoint;
   
   -- x loop 0 - (maxx-1)
   if add_inc = '1' then 
       if x = maxx -1 then
           if y = maxy -1 then
               x := "0000000000000000";
               y := "0000000000000000";
               mhaddr := r.preg.fb_add;
           else
               x := "0000000000000000";
               y := y + '1';
               mhaddr := mhaddr + "100" + r.preg.inc_add;
           end if;
       else
          x := x + '1';
          mhaddr := mhaddr + "100";
       end if;
   end if;  
    
 
    
--   read_en_fifo <= fifo_read;
   read_en_fifo <= '1';
   if fifo_read = '1' then
      read_point := read_point + 1;
      if read_point = fifo_depth then
          read_point := 0;
      end if;
   end if;
   v.creg.fifo_rp :=  std_logic_vector(to_unsigned(read_point,8));
   
-- for the hready assert
   v.creg.dhready := ahbmi.hready;
   
   if  (ahbmi.hready = '1') or (ahbmi.hgrant(mhindex) = '0') then
      v.creg.keep_data := data_out_fifo;
   end if;
   
   if (r.creg.dhready = '0') and (ahbmi.hgrant(mhindex) = '1') then
      vmhwdata := r.creg.keep_data;
   else
      vmhwdata := data_out_fifo;
   end if; 

-- soft reest    
   if vstart = '1' then
      mhaddr := r.preg.fb_add;
      x := (others => '0');
      y := (others => '0');   
      v.hreg.written := '0';
      v.creg.fifo_state := ready;
      v.creg.fifo_rp := (others => '0');
      v.creg.fifo_wp := (others => '0');
      v.creg.mstate := idle;
   end if;

   v.creg.gen_add := mhaddr;
   v.creg.xpoint := x;
   v.creg.ypoint := y;    


-- reset part
   if rst = '0' then
         v.hreg.ycc := (others => '0');
         v.hreg.written := '0';
         v.creg.fifo_state := ready;
         v.creg.fifo_rp := (others => '0');
         v.creg.fifo_wp := (others => '0');
         v.creg.mstate := idle;
         v.creg.xpoint := (others => '0');
         v.creg.ypoint := (others => '0');  
         v.creg.temp_data := (others => '0');
         v.creg.gen_add := (others => '0'); 
   end if;
   
-- signal
      
   rin <= v;   
   apbo.prdata <= vprdata;
   ahbso.hirq <= virq;
   ahbso.hrdata <= vsready & "0000000" & r.hreg.ycc;
   write_pointer_fifo <= r.creg.fifo_wp(5 downto 0);
   read_pointer_fifo <= r.creg.fifo_rp(5 downto 0);
   ahbmo.haddr <= r.creg.gen_add;
   ahbmo.htrans <= mhtrans;
   ahbmo.hbusreq <= mhbusreq;
   ahbmo.hprot <= mhprot;
   ahbmo.hwdata <= vmhwdata; 
   
end process;

   apbo.pirq <= (others => '0');
   apbo.pindex <= pindex;
   apbo.pconfig <= pconfig;
   ahbso.hconfig <= shconfig;
   ahbso.hresp <= "00";
   ahbso.hsplit <= (others => '0');
   ahbso.hcache <= '0';
   ahbso.hready <= '1';
   ahbso.hindex <= shindex;
   ahbmo.hconfig <= mhconfig;
   ahbmo.hlock <= '0';
   ahbmo.hwrite <= '1';
   ahbmo.hsize <= "010";
   ahbmo.hburst <= HBURST_INCR;   
   ahbmo.hirq <= (others => '0');
   ahbmo.hindex <= mhindex;

   
-- registers 
reg : process(clk)
begin
   if rising_edge(clk) then
        r <= rin;
   end if;
end process;


end;
   
