diff --git a/lib/spi/sim/tb_spi_master.wdo b/lib/spi/sim/tb_spi_master.wdo
index 21a71e6..2fb407c 100644
--- a/lib/spi/sim/tb_spi_master.wdo
+++ b/lib/spi/sim/tb_spi_master.wdo
@@ -38,12 +38,14 @@ add wave -noupdate -format Logic /tb_spi_master/inst_spi_master/shift_en
 add wave -noupdate -format Logic /tb_spi_master/inst_spi_master/mosi
 add wave -noupdate -format Literal -radix hexadecimal /tb_spi_master/inst_spi_master/tx_shift_reg
 add wave -noupdate -format Literal -radix hexadecimal /tb_spi_master/inst_spi_master/rx_shift_reg
+add wave -noupdate -format Literal -radix hexadecimal /tb_spi_master/inst_spi_master/rx_shift_reg2
 add wave -noupdate -format Logic /tb_spi_master/inst_spi_master/sclk
 add wave -noupdate -format Literal -radix hexadecimal /tb_spi_master/inst_spi_master/xfer_count
 add wave -noupdate -format Literal -radix hexadecimal /tb_spi_master/inst_spi_master/cmd_reg
 add wave -noupdate -format Literal /tb_spi_master/inst_spi_master/state
 add wave -noupdate -format Logic /tb_spi_master/inst_spi_master/data_load_en
-add wave -noupdate -format Literal /tb_spi_master/inst_spi_master/shift_cnt_pipe
+add wave -noupdate -format Literal /tb_spi_master/inst_spi_master/word_cnt_pipe
+add wave -noupdate -format Logic /tb_spi_master/inst_spi_master/word_cnt_rst
 add wave -noupdate -format Logic /tb_spi_master/inst_spi_master/xfer_count_en
 add wave -noupdate -format Logic /tb_spi_master/inst_spi_master/xfer_count_busy
 add wave -noupdate -format Literal -radix hexadecimal /tb_spi_master/inst_spi_master/data_count
diff --git a/lib/spi/src/spi_master.vhd b/lib/spi/src/spi_master.vhd
index 35292f9..47332f8 100644
--- a/lib/spi/src/spi_master.vhd
+++ b/lib/spi/src/spi_master.vhd
@@ -39,7 +39,7 @@ END spi_master;
 
 ARCHITECTURE behavior OF spi_master IS
 
-	type state_t is (reset, idle, load_data, shift, finish);
+	type state_t is (reset, idle, load_data, shift_write, shift_read, commit, finish);
 
 	signal xfer_count				: unsigned(XFER_MAX_SIZE_BITS-1 downto 0);
 	signal data_count				: unsigned(XFER_MAX_SIZE_BITS-1 downto 0);
@@ -74,11 +74,13 @@ ARCHITECTURE behavior OF spi_master IS
 
 	signal tx_shift_reg				: unsigned(word_width-1 downto 0);
 	signal rx_shift_reg				: unsigned(word_width-1 downto 0);
-	signal shift_cnt_pipe			: unsigned(word_width-1 downto 0);
+	signal rx_shift_reg2			: unsigned(word_width-1 downto 0);
+	
+	signal word_cnt_pipe			: unsigned(word_width-1 downto 0);
+	signal word_cnt_rst				: std_logic;
 	signal data_load_en				: std_logic;
 	signal xfer_start_en			: std_logic;
 	signal ctrl_reg					: ctrl_t;
-	signal rx_enable				: std_logic;
 	signal slv_enable				: std_logic;
 	
 	signal spi_clk_div_en			: std_logic;
@@ -104,7 +106,6 @@ begin
 	status.read_fifo_empty	<= read_fifo_empty;
 	status.rx_valid			<= read_fifo_we;
 	mso						<= slv_enable;
-	rx_enable				<= '1' when cmd_reg.xfer_size > cmd_reg.data_size else '0';
 	
 --------------------------------------------------------------------------
 	-- Instantiate synchronous FIFO
@@ -169,7 +170,7 @@ begin
 		fifo_empty	=>	read_fifo_empty,
 		fifo_afull	=>	open,
 		fifo_aempty	=>	open,
-		data_w		=>  rx_shift_reg,
+		data_w		=>  rx_shift_reg2,
 		data_r		=>  dout
 	);
 
@@ -248,12 +249,13 @@ begin
 end process;
 
 proc_fsm:
-process(state, rx_enable, cat_fifo_empty, cat_fifo_full, write_fifo_empty, write_fifo_full, read_fifo_full, xfer_count_busy, data_count_busy, shift_cnt_pipe, ctrl_reg)
+process(state, cat_fifo_empty, cat_fifo_full, write_fifo_empty, write_fifo_full, read_fifo_full, xfer_count_busy, data_count_busy, word_cnt_pipe, ctrl_reg)
 begin
 	state_next		<= state;
 	
 	xfer_start_en		<= '0';
 	xfer_count_en		<= '0';
+	word_cnt_rst		<= '0';
 	data_load_en		<= '0';
 	clk_en				<= '0';
 	cmd_rdy				<= not cat_fifo_full;
@@ -275,6 +277,7 @@ begin
 		if cat_fifo_empty = '0' then
 			state_next	<= load_data;
 			xfer_start_en	<= '1';
+			word_cnt_rst	<= '1';
 		end if;
 
 	when load_data =>
@@ -282,28 +285,53 @@ begin
 			state_next		<= idle;
 		elsif write_fifo_empty = '0' then
 			data_load_en	 <= '1';
-			state_next		<= shift;
+			state_next		<= shift_write;
 			clk_en			<= ctrl_reg.cpha;
 		end if;
 		
-	when shift =>
+	when shift_write =>
 		clk_en				<= '1';
 		xfer_count_en		<= '1';
 		if xfer_count_busy = '0' then
 			state_next	<= finish;
-		elsif shift_cnt_pipe(shift_cnt_pipe'left) = '1' and data_count_busy = '1' then
-			if write_fifo_empty = '0' then
-				data_load_en 	<= '1';
-			else
-				state_next	<= load_data;
+		elsif data_count_busy = '1' then
+			if word_cnt_pipe(word_cnt_pipe'left) = '1' then
+				if data_count_busy = '1' then
+					if write_fifo_empty = '0' then
+						data_load_en 	<= '1';
+					else
+						state_next	<= load_data;
+					end if;
+				end if;
 			end if;
+		else
+			state_next	<= shift_read;
+			word_cnt_rst	<= '1';
 		end if;
 
+	when shift_read =>
+		clk_en				<= '1';
+		xfer_count_en		<= '1';
+		if xfer_count_busy = '0' or word_cnt_pipe(word_cnt_pipe'left) = '1' then
+			state_next		<= commit;
+		end if;
+	
+	when commit =>
+		clk_en				<= not read_fifo_full and xfer_count_busy;
+		xfer_count_en		<= not read_fifo_full and xfer_count_busy;
+		read_fifo_we		<= '1';
+		if read_fifo_full = '0' then
+			if xfer_count_busy = '0' then
+				state_next		<= finish;
+			else
+				state_next		<= shift_read;
+			end if;
+		end if;
+		
 	when finish =>
-		read_fifo_we	<= rx_enable;
 		cat_fifo_re		<= '1';
 		state_next		<= idle;
-		
+
 	when others =>
 		state_next	<= idle;
 
@@ -327,17 +355,17 @@ begin
 	end if;
 end process;
 
-PROC_SHIFT_COUNT:
+PROC_WORD_COUNT:
 process(spi_clk)
 begin
 	if rising_edge(spi_clk) then
-		if xfer_start_en = '1' then
-			shift_cnt_pipe		<= (shift_cnt_pipe'left downto 1 => '0') & '1';
+		if word_cnt_rst = '1' then
+			word_cnt_pipe		<= (word_cnt_pipe'left downto 1 => '0') & '1';
 		elsif shift_en = '1' and xfer_count_en = '1' then
-			if shift_cnt_pipe(shift_cnt_pipe'left) = '0' then
-				shift_cnt_pipe	<= shift_cnt_pipe(shift_cnt_pipe'left-1 downto 0) & '0';
+			if word_cnt_pipe(word_cnt_pipe'left) = '0' then
+				word_cnt_pipe	<= word_cnt_pipe(word_cnt_pipe'left-1 downto 0) & '0';
 			else
-				shift_cnt_pipe		<= (shift_cnt_pipe'left downto 1 => '0') & '1';
+				word_cnt_pipe	<= (word_cnt_pipe'left downto 1 => '0') & '1';
 			end if;
 		end if;
 	end if;
@@ -394,4 +422,12 @@ process(spi_clk_read)
 		end if;
 	end process;
 
+PROC_RX_SHIFT_REG2:
+process(spi_clk)
+	begin
+		if rising_edge(spi_clk) then
+			rx_shift_reg2	<= rx_shift_reg;
+		end if;
+	end process;
+
 end behavior;
diff --git a/lib/spi/src/tb_spi_master.vhd b/lib/spi/src/tb_spi_master.vhd
index 75364ca..172504b 100644
--- a/lib/spi/src/tb_spi_master.vhd
+++ b/lib/spi/src/tb_spi_master.vhd
@@ -46,7 +46,7 @@ ARCHITECTURE behavior OF tb_spi_master IS
 	signal mst_din			: unsigned(31 downto 0) := (others => '0');
 	signal mst_dout			: unsigned(31 downto 0);
 	signal mst_dout_vld		: STD_LOGIC;
-	signal mst_dout_re		: STD_LOGIC := '1';
+	signal mst_dout_re		: STD_LOGIC := '0';
 	signal mst_shift_en		: STD_LOGIC := '1';
 	signal mosi				: STD_LOGIC;
 	signal miso				: STD_LOGIC := '0';
@@ -103,6 +103,15 @@ CLK_GEN: process
 		CLK <= not CLK;
 	end process;
 
+mst_dout_re_GEN: process
+	begin
+		wait for 10 us;
+		mst_dout_re <= '1';
+		wait for CLK_PERIOD;
+		mst_dout_re <= '1';
+	end process;
+
+
 RX_SLAVE_SIM_SHIFT_REG: process(mst_clk)
 	begin
 		if rising_edge(mst_clk) then