bugfix: sp operation first approach.

[calu.git] / cpu / src / extension_b.vhd

diff --git a/cpu/src/extension_b.vhd b/cpu/src/extension_b.vhd
index 1947bde77ba738377e2817691e3c0acee3d1a8eb..8ffb46a2a21444ca5352a1557e1a2ed9ea332ca5 100644 (file)
--- a/cpu/src/extension_b.vhd
+++ b/cpu/src/extension_b.vhd
@@ -8,24 +8,110 @@ use work.core_pkg.all;
 use work.mem_pkg.all;
 use work.extension_pkg.all;
 
-architecture behav of extension is
+architecture behav of extension_gpm is
 
+type pointers_t is array( 0 to ((2**(paddr_t'length))-1)) of ext_addr_t;
 
-begin
+type gpm_internal is record
+    status : status_rec;
+       preg : pointers_t;
+end record gpm_internal;
+
+signal reg, reg_nxt : gpm_internal;
 
 
-syn: process(clk, reset)
+begin
+syn : process (clk, reset)
+begin
+        if (reset = RESET_VALUE) then
+                reg.status <= (others=>'0');
+               reg.preg <= (others => (std_logic_vector(to_unsigned(DATA_END_ADDR,reg.preg(0)'length))));
+        elsif rising_edge(clk) then
+                reg <= reg_nxt;
+        end if;
+end process syn;
 
+asyn : process (clk, reset, reg, psw_nxt, ext_reg, pwr_en, pinc, paddr)
+       variable reg_nxt_v : gpm_internal;
+       variable incb : gp_register_t;
+       variable sel_pval : std_logic_vector(reg.preg(0)'range);
+       
+       variable data_out_v : gp_register_t;
+       variable data_v : gp_register_t;
+       variable tmp_data  : gp_register_t;
 begin
+       reg_nxt_v := reg;
+       data_v  := ext_reg.data;
+
+       psw <= reg.status;
+       
+       data_out_v := (others => '0');
 
-       if (reset = RESET_VALUE) then
-               
-       elsif rising_edge(clk) then
-               
+       incb(0) := '1';
+       if pinc = '1' then
+               incb(incb'high downto 1) := (others => '1');
+       else
+               incb(incb'high downto 1) := (others => '0');
        end if;
        
-end process; 
+       if (ext_reg.sel = '1') and ext_reg.wr_en = '1' then
+               case ext_reg.addr(1 downto 0) is
+               when "00" => 
+                       if ext_reg.byte_en(0) = '1' then
+                               reg_nxt_v.status := (data_v(0), data_v(1), data_v(3), data_v(2));
+                               psw <= reg_nxt_v.status;
+                       end if;
+               when "01" =>
+                       --STACK_POINTER
+                       tmp_data := (others =>'0');
+                       tmp_data(tmp_data'high downto BYTEADDR) := reg.preg(0);
+                       
+                       if ext_reg.byte_en(0) = '1' then
+                               tmp_data(byte_t'range) := data_v(byte_t'range);
+                       end if;
+                       if ext_reg.byte_en(1) = '1' then
+                               tmp_data((2*byte_t'length-1) downto byte_t'length) := data_v((2*byte_t'length-1) downto byte_t'length);
+                       end if;
+                       if ext_reg.byte_en(2) = '1' then
+                               tmp_data((3*byte_t'length-1) downto 2*byte_t'length) := data_v((3*byte_t'length-1) downto 2*byte_t'length);
+                       end if;
+                       if ext_reg.byte_en(3) = '1' then
+                               tmp_data((4*byte_t'length-1) downto 3*byte_t'length) := data_v((4*byte_t'length-1) downto 3*byte_t'length);
+                       end if;
+                       
+                       reg_nxt_v.preg(0) := tmp_data(tmp_data'high downto BYTEADDR);
+               when others => null;
+               end case;
+       end if;
+       
+       
+       if (ext_reg.sel = '1') and ext_reg.wr_en = '0' then
+               case ext_reg.addr(1 downto 0) is
+               when "00" => 
+                       if ext_reg.byte_en(0) = '1' then
+                               data_out_v(3 downto 0) := (reg.status.sign, reg.status.carry, reg.status.oflo, reg.status.zero);
+                       end if;
+               when "01" =>
+                       --STACK_POINTER
+                       data_out_v(data_out_v'high downto BYTEADDR) := reg.preg(0);
+               when others => null;
+               end case;
+       end if;
 
+       sel_pval:= reg_nxt_v.preg(to_integer(unsigned(paddr)));
+       
+       if pwr_en = '1' then
+               reg_nxt_v.preg(to_integer(unsigned(paddr))) := std_logic_vector(unsigned(sel_pval)+unsigned(incb));
+       end if;
+
+       reg_nxt_v.status := psw_nxt;
+       
+       reg_nxt <= reg_nxt_v;
+       data_out <= data_out_v;
+       
+       pval <= (others =>'0');
+       pval(pval'high downto BYTEADDR) <= sel_pval;
+end process asyn;
 
 end behav;