spi_core is designed now

10 files changed, 600 insertions, 439 deletions

diff --git a/fifo.v b/fifo.v
index a119aa2..35e1691 100644
--- a/fifo.v
+++ b/fifo.v
@@ -2,18 +2,18 @@ module fifo #(
     parameter NUM_WORDS = 128,
     parameter DATA_W    = 32
 ) (
-    input              clk_i,
-    input              rst_i,
+    input                  clk_i,
+    input                  rst_i,
 
     // Write side
-    input              wr_i,
-    input [DATA_W-1:0] data_i,
-    output             full_o,
+    input                  wr_i,
+    input [DATA_W-1:0]     data_i,
+    output                 full_o,
 
     // Read side
-    input              rd_i,
+    input                  rd_i,
     output reg[DATA_W-1:0] data_o,
-    output             empty_o
+    output                 empty_o
 );
 
 //***********************************
diff --git a/rom_ctrl.v b/rom_ctrl.v
index ebc887c..11d63b5 100644
--- a/rom_ctrl.v
+++ b/rom_ctrl.v
@@ -16,12 +16,6 @@ wire read_buf_empty, write_buf_empty, read_buf_full, write_buf_full;
 wire read_buf_rst, write_buf_rst;
 wire[31:0] read_buf_data_i, write_buf_data_i, read_buf_data_o, write_buf_data_o;
 
-reg spi_writer_wr;
-wire spi_writer_tx_need;
-wire[23:0] spi_writer_addr;
-
-wire spi_finish;
-
 //***********************************
 // Controller state
 //***********************************
@@ -30,9 +24,6 @@ wire spi_finish;
 reg[4:0] state = IDLE_S;
 reg[4:0] next_state;
 
-// ACK 
-assign ack_o             = ( state == WAIT_S ? 0 : 1 );
-
 // List of states
 localparam IDLE_S                    = 0,
            WAIT_S                    = 1,
@@ -82,9 +73,6 @@ assign write_buf_data_i = dat_i;
 // Write to the WRITE BUFFER when write data to WDATA
 assign write_buf_wr = ( state == PUSHING_WRITE_BUFFER_S );
 
-// Read data from WRITE BUFFER when write data to SPI controller 
-assign write_buf_rd = ( state == WRITING_DATA_S && spi_writer_tx_need );
-
 fifo write_buf(
   .clk_i(clk_i),
   .rst_i(write_buf_rst),
@@ -121,45 +109,28 @@ fifo read_buf(
 );
 
 //***********************************
-// M25P16 SPI Writer Core
+// M25P16 SPI Core
 //***********************************
 
-assign spi_writer_tx      = write_buf_data_o;
-assign spi_writer_addr    = registers[WADDR][23:0];
-
-/*
-spi_writer spi_writer(
-  .clk_i(clk_i),
-  .tx_i(spi_writer_tx),
-  .adr_i(spi_writer_addr),
-  .wr_i(spi_writer_wr),
-  .tx_need(spi_writer_tx_need),
-  .finish_o(spi_finish),
+wire spi_finish;
+wire[23:0] spi_addr;
+assign spi_addr = dat_i[23:0];
 
-  .spi_mosi()
-  .spi_miso()
-);*/
+reg[4:0] spi_opcode = 0;
 
-//***********************************
-// M25P16 SPI Reader Core
-//***********************************
+m25p16_spi_core spi(
+  .clk_i( clk_i ),
+  .opcode_i( spi_opcode ),
 
-assign spi_writer_tx      = write_buf_data_o;
-assign spi_writer_addr    = registers[WADDR][23:0];
+  .dat_i( write_buf_data_o ),
+  .dat_o( read_buf_data_i ),
+  .dat_o_read( write_buf_rd ),
+  .dat_o_valid( read_buf_wr ),
 
-/*
-spi_reader spi_reader(
-  .clk_i(clk_i),
-  .tx_i(spi_writer_tx),
-  .adr_i(spi_writer_addr),
-  .wr_i(spi_writer_wr),
-  .dat_o(read_buf_wr)
-  .dat_valid(
-  .finish_o(spi_finish)
+  .adr_i( spi_addr ),
 
-  .spi_mosi()
-  .spi_miso()
-);*/
+  .finish_o(spi_finish),
+);
 
 //***********************************
 // Virtual Registers I/O
@@ -172,7 +143,8 @@ always @( posedge clk_i )
 always @( posedge clk_i )
   if( state == WRITING_REG_S || 
       adr_i == RADDR || 
-      adr_i == WADDR )
+      adr_i == WADDR || 
+      adr_i == SEC_ADDR_ERASE )
     registers[adr_i] <= dat_i;
 
 //***********************************
@@ -186,6 +158,24 @@ always @( posedge clk_i, posedge rst_i )
     state <= next_state;
 
 always @*
+  case( next_state )
+    READING_DATA_S:
+      spi_opcode = 1;
+    WRITING_DATA_S:
+      spi_opcode = 2;
+    ERASING_SECTOR_S:
+      spi_opcode = 3;
+    BULK_ERASING_S:
+      spi_opcode = 4;
+    READING_STATUS_S:
+      spi_opcode = 5;
+    WRITING_STATUS_S:
+      spi_opcode = 6;
+    default:
+      spi_opcode = 0;
+  endcase
+
+always @*
   begin
     next_state = state;
     case( state )
@@ -208,40 +198,42 @@ always @*
             next_state = CLEARING_WRITE_BUFFER_S;
 
           else if( adr_i == WLEN  && wr_i)
-          	next_state = WRITING_DATA_S;
+            next_state = WRITING_DATA_S;
 
 
           else if( wr_i && adr_i == SEC_ADDR_ERASE )
-          	next_state = ERASING_SECTOR_S;
+            next_state = ERASING_SECTOR_S;
 
           else if( wr_i && adr_i == BULK_ERASE )
-          	next_state = BULK_ERASING_S;
+            next_state = BULK_ERASING_S;
 
           else if( rd_i && adr_i == STATUS )
-          	next_state = READING_STATUS_S;
+            next_state = READING_STATUS_S;
 
           else if( wr_i && adr_i == STATUS )
-          	next_state = WRITING_STATUS_S;
-
+            next_state = WRITING_STATUS_S;
 
           else if( wr_i )
-          	next_state = WRITING_REG_S;
+            next_state = WRITING_REG_S;
 
           else if( rd_i )
-          	next_state = READING_REG_S;
+            next_state = READING_REG_S;
         end  
 
-    ERASING_SECTOR_S, BULK_ERASING_S, READING_STATUS_S, WRITING_STATUS_S, READING_DATA_S, WRITING_DATA_S:
+      ERASING_SECTOR_S, BULK_ERASING_S, READING_STATUS_S, WRITING_STATUS_S, READING_DATA_S, WRITING_DATA_S:
         if( spi_finish )
           next_state = WAIT_S;
 
-    POPPING_READ_BUFFER_S, CLEARING_READ_BUFFER_S, CLEARING_WRITE_BUFFER_S, PUSHING_WRITE_BUFFER_S, READING_REG_S, WRITING_REG_S:
+      POPPING_READ_BUFFER_S, CLEARING_READ_BUFFER_S, CLEARING_WRITE_BUFFER_S, PUSHING_WRITE_BUFFER_S, READING_REG_S, WRITING_REG_S:
         next_state = WAIT_S;
 
-    WAIT_S:
+      WAIT_S:
         next_state = IDLE_S;
 
-	  endcase
+    endcase
   end  
 
+// ACK 
+assign ack_o                         = ( state == WAIT_S ? 0 : 1 );
+
 endmodule
diff --git a/rom_wb.v b/rom_wb.v
index c0b6c6c..8062f86 100644
--- a/rom_wb.v
+++ b/rom_wb.v
@@ -22,35 +22,35 @@ wire rom_clk_i, rom_read_i, rom_write_i;
 
 assign rom_read_i  = wb_cyc_i & wb_stb_i & !wb_we_i;
 assign rom_write_i = wb_cyc_i & wb_stb_i & wb_we_i;
-assign rom_dat_i = wb_dat_i;
-assign rom_addr_i = wb_adr_i;
-assign rom_clk_i = wb_clk_i;
+assign rom_dat_i   = wb_dat_i;
+assign rom_addr_i  = wb_adr_i;
+assign rom_clk_i   = wb_clk_i;
 
 rom_ctrl rom_ctrl(
-   .clk_i(rom_clk_i), 
-   .rst_i(0),
-
-   .adr_i(rom_addr_i),
-
-   .rd_i(rom_read_i),
-   .dat_o(rom_dat_o),
-
-   .wr_i(rom_write_i),
-   .dat_i(rom_dat_i),
-
-   .ack_o(ack_w)
+  .clk_i(rom_clk_i), 
+  .rst_i(0),
+  
+  .adr_i(rom_addr_i),
+  
+  .rd_i(rom_read_i),
+  .dat_o(rom_dat_o),
+  
+  .wr_i(rom_write_i),
+  .dat_i(rom_dat_i),
+  
+  .ack_o(ack_w)
 );
 
 always @(posedge wb_clk_i, posedge wb_rst_i )
-	if( wb_rst_i )
-		ack_d1 <= 0;
-	else
-		ack_d1 <= ack_w;
-		
+  if( wb_rst_i )
+    ack_d1 <= 0;
+  else
+    ack_d1 <= ack_w;
+
 // если контроллер был занят, а стал свободен, значит контроллер завершил операцию и
 // нужно дёрнуть один раз ACK_O и положить на следующем такте, чтобы wishbone
 // master оповестился о конце операции slave'а
 assign wb_ack_o = ( ack_d1 ) && ( !ack_w );
 assign wb_dat_o = rom_dat_o;
 
-endmodule
\ No newline at end of file
+endmodule
diff --git a/romctrl_testbench.v b/romctrl_testbench.v
index 9c4b1cf..f816abe 100644
--- a/romctrl_testbench.v
+++ b/romctrl_testbench.v
@@ -1,122 +1,105 @@
 `timescale 1ns / 1ps
 
-////////////////////////////////////////////////////////////////////////////////
-// Company: 
-// Engineer:
-//
-// Create Date:   13:56:22 12/25/2015
-// Design Name:   rom_ctrl
-// Module Name:   /home/sn/src/circuit-design-lab234//romctrl-testbench.v
-// Project Name:  m25p16
-// Target Device:  
-// Tool versions:  
-// Description: 
-//
-// Verilog Test Fixture created by ISE for module: rom_ctrl
-//
-// Dependencies:
-// 
-// Revision:
-// Revision 0.01 - File Created
-// Additional Comments:
-// 
-////////////////////////////////////////////////////////////////////////////////
-
 module romctrl_testbench;
 
-	// Inputs
-	reg clk_i;
-	reg rst_i;
-	reg rd_i;
-	reg wr_i;
-	reg [31:0] dat_i;
-	reg [4:0] adr_i;
+reg clk_i;
+reg rst_i;
+reg rd_i;
+reg wr_i;
+reg [31:0] dat_i;
+reg [4:0] adr_i;
 
-	// Outputs
-	wire [31:0] dat_o;
-	wire ack_o;
+wire [31:0] dat_o;
+wire ack_o;
 
-	// Instantiate the Unit Under Test (UUT)
-	rom_ctrl uut (
-		.clk_i(clk_i), 
-		.rst_i(rst_i), 
-		.rd_i(rd_i), 
-		.wr_i(wr_i), 
-		.dat_i(dat_i), 
-		.adr_i(adr_i), 
-		.dat_o(dat_o), 
-		.ack_o(ack_o)
-	);
+rom_ctrl uut (
+  .clk_i(clk_i), 
+  .rst_i(rst_i), 
+  .rd_i(rd_i), 
+  .wr_i(wr_i), 
+  .dat_i(dat_i), 
+  .adr_i(adr_i), 
+  .dat_o(dat_o), 
+  .ack_o(ack_o)
+);
 
 initial begin
-	clk_i = 0;
-	forever begin
-		clk_i = !clk_i;
-		#50;
-	end
+  clk_i = 0;
+  forever begin
+    clk_i = !clk_i;
+    #50;
+  end
 end
 
-reg [31:0] read_value;	
-initial begin
-	rst_i = 0;
-	
-	clear();
-	
-	$display("hello world");
-	
-	write( 5'hF, 32'haabbccdd );
-	read( 5'hF,  read_value );
-	
-	$display("read_value = 0x%h", read_value);
-	
-	clear();
+reg [31:0] read_value;
+localparam ADR = 5'hF,
+           VAL = 32'habbccdd;
+
 	
-	$finish();
-end
+initial begin
+  rst_i = 0;
+  
+  clear();
+  
+  $display( "ROM Controller Testbench started." );
+  
+  $display( "Writing to ROM value 0x%h by address 0x%h...", ADR, VAL );
+  write( ADR, VAL );
+  $display( "Writing finished.", );
 
-task write( input [4:0] adr, input [31:0] dat );
-begin
-		@( posedge clk_i );
-		
-		adr_i <= adr;
-		dat_i <= dat;
-		wr_i  <= 1;
-		rd_i  <= 0;
+  $display( "Reading from ROM from address 0x%h...", ADR );
+  read( ADR, read_value );
+  $display( "Read value is 0x%h", read_value );
+  
+  $display( "ROM Controller Testbench finished." );
 
-		@( posedge clk_i );
-		while( ack_o == 1'b1 )
-		  @( posedge clk_i );
-		  
-		wr_i  <= 0;
-		rd_i  <= 0;
+  clear();
+  
+  $finish();
 end
+
+task write( input [4:0] adr, input [31:0] dat );
+  begin
+    @( posedge clk_i );
+    
+    adr_i <= adr;
+    dat_i <= dat;
+    wr_i  <= 1;
+    rd_i  <= 0;
+    
+    @( posedge clk_i );
+    while( ack_o == 1'b1 )
+      @( posedge clk_i );
+      
+    wr_i  <= 0;
+    rd_i  <= 0;
+  end
 endtask
 
 task read( input [4:0] adr, output [31:0] dat );
-begin		
-		adr_i <= adr;
-		wr_i  <= 0;
-		rd_i  <= 1;
-
-		@( posedge clk_i );
-		while( ack_o == 1'b1 )
-		  @( posedge clk_i );
-		  
-		dat = dat_o;		  
-		wr_i  <= 0;
-		rd_i  <= 0;
-end
+  begin		
+    adr_i <= adr;
+    wr_i  <= 0;
+    rd_i  <= 1;
+    
+    @( posedge clk_i );
+    while( ack_o == 1'b1 )
+      @( posedge clk_i );
+      
+    dat = dat_o;		  
+    wr_i  <= 0;
+    rd_i  <= 0;
+  end
 endtask
 
 task clear();
-begin
-		@( posedge clk_i );
-		rd_i <= 0;
-		wr_i <= 0;
-		dat_i <= 0;
-		adr_i <= 0;
-end
+  begin
+    @( posedge clk_i );
+    rd_i <= 0;
+    wr_i <= 0;
+    dat_i <= 0;
+    adr_i <= 0;
+  end
 endtask
       
 endmodule
-
diff --git a/romwb_testbench.v b/romwb_testbench.v
index 926d449..f221533 100644
--- a/romwb_testbench.v
+++ b/romwb_testbench.v
@@ -15,97 +15,103 @@ wire [31:0] wb_dat_o;
 wire wb_ack_o;
 
 rom_wb uut (
-	.wb_clk_i(wb_clk_i), 
-	.wb_rst_i(wb_rst_i), 
-	.wb_dat_i(wb_dat_i), 
-	.wb_adr_i(wb_adr_i), 
-	.wb_we_i(wb_we_i), 
-	.wb_sel_i(wb_sel_i), 
-	.wb_cyc_i(wb_cyc_i), 
-	.wb_stb_i(wb_stb_i), 
-	.wb_dat_o(wb_dat_o), 
-	.wb_ack_o(wb_ack_o)
+  .wb_clk_i(wb_clk_i), 
+  .wb_rst_i(wb_rst_i), 
+  .wb_dat_i(wb_dat_i), 
+  .wb_adr_i(wb_adr_i), 
+  .wb_we_i(wb_we_i), 
+  .wb_sel_i(wb_sel_i), 
+  .wb_cyc_i(wb_cyc_i), 
+  .wb_stb_i(wb_stb_i), 
+  .wb_dat_o(wb_dat_o), 
+  .wb_ack_o(wb_ack_o)
 );
 
 initial begin
-	wb_clk_i = 0;
-	forever begin
-		wb_clk_i = !wb_clk_i;
-		#50;
-	end
+  wb_clk_i = 0;
+  forever begin
+    wb_clk_i = !wb_clk_i;
+    #50;
+  end
 end
 
-reg [31:0] read_value;	
-initial begin
-	wb_sel_i = 0;
-	wb_rst_i = 0;
-	 
-	clear();
-	
-	$display("hello world");
-	
-	write( 5'h4, 32'haabbccdd );
-	read( 5'h4,  read_value );
-	
-	$display("read_value = 0x%h", read_value);
+reg [31:0] read_value;
+localparam ADR = 5'hF,
+           VAL = 32'habbccdd;
 	
-	clear();
-	
-	$finish();
+initial begin
+  wb_sel_i = 0;
+  wb_rst_i = 0;
+  
+  clear();
+  
+  $display( "ROM Wishbone Controller Testbench started." );
+  
+  $display( "Writing to ROM value 0x%h by address 0x%h...", ADR, VAL );
+  write( ADR, VAL );
+  $display( "Writing finished.", );
+
+  $display( "Reading from ROM from address 0x%h...", ADR );
+  read( ADR, read_value );
+  $display( "Read value is 0x%h", read_value );
+  
+  $display( "ROM Wishbone Controller Testbench finished." );
+
+  clear();
+  
+  $finish();
 end
 
 task write( input [4:0] adr, input [31:0] dat );
-begin
-		@( posedge wb_clk_i );
-		
-  		wb_adr_i <= adr; // master set ADR to 4 for WRITING to ADR
-		wb_we_i  <= 1;   // master set WE_I to 1 for WRITING
-		wb_stb_i <= 1;   // master set STB_O to 1 for indicate of start phase
-		wb_cyc_i <= 1;   // master set CYC_O to 1 for indicate of start cycle
-		wb_dat_i <= dat;
-		
-		@( posedge wb_clk_i );
-		while( wb_ack_o == 1'b0 )
-		  @( posedge wb_clk_i );
-		  
-		wb_stb_i <= 1'b0;
-		wb_cyc_i <= 1'b0;
-end
+  begin
+    @( posedge wb_clk_i );
+    
+    wb_adr_i <= adr; // master set ADR to 4 for WRITING to ADR
+    wb_we_i  <= 1;   // master set WE_I to 1 for WRITING
+    wb_stb_i <= 1;   // master set STB_O to 1 for indicate of start phase
+    wb_cyc_i <= 1;   // master set CYC_O to 1 for indicate of start cycle
+    wb_dat_i <= dat;
+    
+    @( posedge wb_clk_i );
+    while( wb_ack_o == 1'b0 )
+      @( posedge wb_clk_i );
+      
+    wb_stb_i <= 1'b0;
+    wb_cyc_i <= 1'b0;
+  end
 endtask
 
 task read( input [4:0] adr, output [31:0] dat );
-begin
-		@( posedge wb_clk_i );
-		
-		wb_adr_i <= adr; // master set ADR to 4 for READING from ADR
-		wb_we_i  <= 0;   // master set WE_I to 0 for READING
-		wb_stb_i <= 1;   // master set STB_O to 1 for indicate of start phase
-		wb_cyc_i <= 1;   // master set CYC_O to 1 for indicate of start cycle
-		
-		@( posedge wb_clk_i );
-		while( wb_ack_o == 1'b0 )
-		  @( posedge wb_clk_i );
-		  
-		dat <= wb_dat_o;
-		
-		wb_stb_i <= 1'b0;
-		wb_cyc_i <= 1'b0;
-end
+  begin
+    @( posedge wb_clk_i );
+    
+    wb_adr_i <= adr; // master set ADR to 4 for READING from ADR
+    wb_we_i  <= 0;   // master set WE_I to 0 for READING
+    wb_stb_i <= 1;   // master set STB_O to 1 for indicate of start phase
+    wb_cyc_i <= 1;   // master set CYC_O to 1 for indicate of start cycle
+    
+    @( posedge wb_clk_i );
+    while( wb_ack_o == 1'b0 )
+      @( posedge wb_clk_i );
+      
+    dat <= wb_dat_o;
+    
+    wb_stb_i <= 1'b0;
+    wb_cyc_i <= 1'b0;
+  end
 endtask
 
 task clear();
-begin
-		@( posedge wb_clk_i );
-		
-		wb_adr_i <= 0;
-		wb_we_i  <= 0;
-		wb_stb_i <= 0;
-		wb_cyc_i <= 0;
-		wb_sel_i = 0;
-		wb_rst_i = 0;
-end
+  begin
+    @( posedge wb_clk_i );
+    
+    wb_adr_i <= 0;
+    wb_we_i  <= 0;
+    wb_stb_i <= 0;
+    wb_cyc_i <= 0;
+    wb_sel_i = 0;
+    wb_rst_i = 0;
+  end
 endtask
       
 endmodule
-
-
diff --git a/spi_core.v b/spi_core.v
new file mode 100644
index 0000000..1b6375e
--- /dev/null
+++ b/spi_core.v
@@ -0,0 +1,156 @@
+`timescale 1ns / 1ps
+
+module m25p16_spi_core(
+  input          clk_i,
+  input          rst_i,
+
+  input  [4:0]   opcode_i,
+  input  [31:0]  info_i,
+  input  [31:0]  dat_i,
+
+  output [31:0]  dat_o,
+  output         dat_o_read,
+  output         dat_o_valid,
+
+  input  [31:0]  adr_i,
+  output         finish_o
+);
+
+wire spi_finish;
+
+reg [31:0] data; 
+reg [31:0] info;
+reg [7:0]  cnt;
+
+//***********************************
+// SPI M25P16 Core state
+//***********************************
+
+// Memory for storing state 
+reg[7:0] state = IDLE_S;
+reg[7:0] next_state;
+
+localparam OPCODE_READING_DATA   = 1,
+           OPCODE_WRITING_DATA   = 2,
+           OPCODE_ERASING_SECTOR = 3,
+           OPCODE_BULK_ERASING   = 4,
+           OPCODE_READING_STATUS = 5,
+           OPCODE_WRITING_STATUS = 6;
+
+localparam IDLE_S                       = 0,
+           WRITING_DATA_WRITE_ENABLE_S  = 1,
+           WRITING_DATA_WRITE_INSTR_S   = 2,
+           WRITING_DATA_WRITE_DATA_S    = 3,
+           WRITING_DATA_WRITE_DISABLE_S = 4,
+           FINISH_S                     = 64;
+
+always @*
+  case( state )
+    IDLE_S:
+      info = info_i;
+  endcase
+  
+always @( posedge clk_i, posedge rst_i )
+  if( rst_i ) 
+    state <= IDLE_S;
+  else
+    state <= next_state;
+
+always @*
+  begin
+    next_state = state;
+    case( state )
+      IDLE_S: 
+        if( opcode_i == OPCODE_WRITING_DATA )
+          next_state = WRITING_DATA_WRITE_ENABLE_S;
+      WRITING_DATA_WRITE_ENABLE_S:
+        if( spi_finish ) 
+          next_state = WRITING_DATA_WRITE_INSTR_S;
+      WRITING_DATA_WRITE_INSTR_S:
+        if( next_w )
+          next_state = WRITING_DATA_WRITE_DATA_S;
+      WRITING_DATA_WRITE_DATA_S:
+        if( spi_finish )
+          next_state = WRITING_DATA_WRITE_DISABLE_S;
+      WRITING_DATA_WRITE_DISABLE_S:
+        if( spi_finish )
+          next_state = FINISH_S;
+      FINISH_S:
+        next_state = IDLE_S;
+    endcase
+  end  
+
+always @*
+  case( next_state )
+    WRITING_DATA_WRITE_INSTR_S:
+      cnt = 4;
+
+    WRITING_DATA_WRITE_ENABLE_S, WRITING_DATA_WRITE_DISABLE_S:
+      cnt = 1;
+
+    WRITING_DATA_WRITE_DATA_S:
+      cnt = info[7:0] << 2;
+  endcase
+
+always @*
+  case( state )
+    WRITING_DATA_WRITE_DATA_S:
+      data = dat_i;
+  endcase
+
+always @*
+  case( next_state )
+    WRITING_DATA_WRITE_ENABLE_S:
+      data = 32'hAA;
+    WRITING_DATA_WRITE_INSTR_S:
+      data = 32'hFF000000 | adr_i[23:0];
+    WRITING_DATA_WRITE_DATA_S: 
+      data = dat_i;
+    WRITING_DATA_WRITE_DISABLE_S:
+      data = 32'hDD;
+  endcase
+    
+//***********************************
+// SPI Routine
+//***********************************
+
+wire spi_miso_i, spi_sclk_o, spi_mosi_o, spi_ss_i;
+
+assign spi_rd_i   = 1'b0; // TODO: unused
+assign spi_wr_i   = (WRITING_DATA_WRITE_ENABLE_S  ||
+                    WRITING_DATA_WRITE_INSTR_S   ||
+                    WRITING_DATA_WRITE_DATA_S    ||
+                    WRITING_DATA_WRITE_DISABLE_S);
+
+assign spi_sclk_i = clk_i;
+
+wire [31:0] rx_o; // TODO: unused
+
+wire next_o;
+wire next_w;
+
+assign next_o = ( state == WRITING_DATA_WRITE_DATA_S ? dat_o_read : next_w );
+
+spi_routine spi_routine(
+  .clk_i(clk_i),
+  .rst_i(1'b0),
+
+  .sclk_o(spi_sclk_o),
+  .ss_i(spi_ss_i),
+  .mosi_o(spi_mosi_o),
+  .miso_i(spi_miso_i),
+
+  .rx_o(rx_o),
+  .tx_i(data),
+
+  .wr_i(spi_wr_i),
+  .rd_i(spi_rd_i),
+  .cnt_i(cnt),
+
+  .next_o(next_o),
+  .finish_o(spi_finish)
+);
+
+assign finish_o   = ( state == FINISH_S ? 1 : 0 );
+
+endmodule
diff --git a/spi_routine.v b/spi_routine.v
index 3733e50..2d22cfe 100644
--- a/spi_routine.v
+++ b/spi_routine.v
@@ -2,91 +2,107 @@
 
 module spi_routine
 (
-  input [7:0] tx_i,
-  output [7:0] rx_o,
-  
-  input rst_i,
-  
-  input miso_i,
   input clk_i,
-  input ss_i,
-  output reg mosi_o,
-  output sclk_o
+  input rst_i,
 
+  input [31:0] tx_i,
+  output reg [31:0] rx_o,
+ 
   input rd_i,
   input wr_i,
+  input[7:0] cnt_i,
 
-  output finish_o
+  input miso_i,
+  input ss_i,
+
+  output reg mosi_o,
+  output finish_o,
+  output sclk_o,
+  output next_o
 );
 
 localparam ADDR_WIDTH = 3;
-	
-reg in_progress = 0;
 
 reg [ADDR_WIDTH-1:0] idx = 0;
-reg [1:0] start = 0;
 
-
-reg [2:0] state;
+reg [2:0] state = IDLE_S;
 reg [2:0] next_state;
+reg [7:0] current_cnt;
+reg [1:0] sel = 0;
 
+reg [31:0] saved_tx;
+wire [7:0] tx;
 
 wire finish_w;
 
 localparam READ_S  = 3'd2,
            WRITE_S = 3'd1,
-		       IDLE_S  = 3'd0;
+           IDLE_S  = 3'd0;
+
+assign tx = ( sel == 0 ? saved_tx[7:  0] :
+            ( sel == 1 ? saved_tx[15: 8] : 
+            ( sel == 2 ? saved_tx[23:16] :
+                         saved_tx[31:24])));
+	 
+always @*
+  if( sel == 0 )
+    saved_tx = tx_i;
 
 always @(posedge clk_i, posedge rst_i )
   if( rst_i )
     state <= IDLE_S;
   else
     state <= next_state;
-	 
+
 always @*
 begin
   next_state = state;
-  case( state ):
-	 IDLE_S:
-	   begin
-		  if( rd_i )
-		    next_state = READ_S;
-		  else 
-		    if( wr_i )
-		      next_state = WRITE_S;
-		end
-	 READ_S:
-	   begin
-		  if( finish_w )
-		    next_state = IDLE_S;
-		end
-	 WRITE_S:
-	   begin
-		  if( finish_w )
-		    next_state = IDLE_S;
-		end
+  case( state )
+    IDLE_S:
+      if( rd_i )
+        next_state = READ_S;
+      else 
+        if( wr_i )
+          next_state = WRITE_S;
+    READ_S:
+      if( finish_w )
+        next_state = IDLE_S;
+    WRITE_S:
+      if( finish_w )
+        next_state = IDLE_S;
   endcase
 end
 
 always @*
-  if( state == WRITE_S )
-    mosi <= tx_i[idx];
-  else
-    mosi <= 1'b1;
+  if( state == IDLE_S )
+    begin
+      current_cnt = 0;
+      sel = 0;
+    end
+
+always @*
+  mosi_o <= ( state == WRITE_S ? tx[idx] : 1'b1 );
 
-reg [7:0] rx;
-always @(posedge clk)
-  if( state == READ_S )
-    rx[idx] <= miso;
+always @( posedge clk_i )
+  rx_o[idx] <= miso_i;
   
-always @(posedge clk)
-	if( state == IDLE_S )
-		idx <= 2'd0;
-	else
-		idx <= idx + 2'd1;
+always @( posedge clk_i )
+  if( state == IDLE_S )
+    idx <= 2'd0;
+  else
+    idx <= idx + 2'd1;
+
+always @( posedge clk_i )
+  if( idx == 7 ) 
+    begin
+      current_cnt <= current_cnt + 1;
+      sel         <= sel + 1;
+    end
 
-assign finish_w = (idx == 7);
+assign finish_w = ( state != IDLE_S && current_cnt == cnt_i ? 1 : 0 );
 
 assign finish_o = finish_w;
+assign sclk_o   = clk_i;
+assign next_o   = ( state != IDLE_S && sel == 3 && current_cnt != cnt_i-1 ? 1 : 0 );
 
 endmodule
diff --git a/spi_writer.v b/spi_writer.v
deleted file mode 100644
index 19db035..0000000
--- a/spi_writer.v
+++ /dev/null
@@ -1,129 +0,0 @@
-`timescale 1ns / 1ps
-
-module spi_writer(
-   input clk_i,
-   input[7:0]  tx_i,
-   input[23:0] adr_i,
-   input  wr_i,
-   output tx_need,
-   output finish_o
-);
-
-wire spi_miso_i, spi_sclk_i, spi_mosi_o, spi_ss_i;
-
-reg [7:0] spi_tx;
-reg [7:0] spi_rx;
-
-//***********************************
-// SPI M25P16 Writer state
-//***********************************
-
-// Memory for storing state 
-reg[4:0] state;
-
-localparam IDLE = 0,
-           WRITE_ENABLE  = 1,
-           WRITE_INSTR   = 2,
-           WRITE_ADDR    = 3,
-           WRITE_DATA    = 4,
-           WRITE_DISABLE = 5;
-
-//***********************************
-// SPI Routine
-//***********************************
-
-assign spi_rd_i = 0;
-assign spi_sclk_i = clk_i;
-
-wire spi_finish;
-
-spi_routine (
-  .clk_i(spi_sclk_i),
-
-  .ss_i(spi_ss_i),
-  .mosi_o(spi_mosi_o),
-  .miso_i(spi_miso_i),
-
-  .rx_o(spi_rx),
-  .tx_i(spi_tx),
-
-  .rd_i(spi_rd_i),
-  .wr_i(spi_wr_i),
-
-  .finish_o(spi_finish)
-);
-
-always @(posedge clk_i)
-begin
-  case(state)
-    IDLE:
-    begin
-      if( wr_i ) begin
-        state <= WRITE_ENABLE;
-        spi_wr_i <= 0;
-        spi_ss_i <= 1;
-      end
-    end
-
-    WRITE_ENABLE:
-	  begin
-      if( spi_finish ) begin
-        state <= WRITE_INSTR;
-        spi_ss_i <= 1;
-      end else begin
-        spi_wr_i <= 1;
-        spi_tx <= 8'h00;
-        spi_ss_i <= 0;
-      end
-	  end
-
-    WRITE_INSTR:
-	  begin
-      if( spi_finish )
-        state <= WRITE_ADDR;
-      else begin
-        spi_tx <= 8'h00;
-        spi_wr_i <= 1;
-        spi_ss_i <= 0;
-      end
-	  end
-
-	  WRITE_ADDR:
-	  begin
-      if( spi_finish ) begin
-        state = WRITE_DATA;
-      end else begin
-        spi_tx <= 8'h00;
-        spi_wr_i <= 1;
-        spi_ss_i <= 0;
-      end
-	  end
-
-    WRITE_DATA:
-	  begin
-      if( spi_finish && !wr_i ) begin
-        state    <= WRITE_DISABLE;
-        spi_ss_i <= 1;
-      end else if( spi_finish && wr_i ) begin
-        tx_need  <= 1;
-      end else if( !spi_finish && wr_i ) begin
-        spi_tx   <= tx_i;
-        spi_ss_i <= 0;
-      end
-	  end
-
-	  WRITE_DISABLE:
-	  begin
-      if( spi_finish ) begin
-        state <= IDLE;
-        spi_ss_i <= 1;
-        finish_o <= 1;
-      end else begin
-        spi_tx <= 8'h00;
-        spi_wr_i <= 1;
-        spi_ss_i <= 0;
-      end
-	  end
-end
-
-endmodule
diff --git a/spicore_testbench.v b/spicore_testbench.v
new file mode 100644
index 0000000..09db82a
--- /dev/null
+++ b/spicore_testbench.v
@@ -0,0 +1,58 @@
+`timescale 1ns / 1ps
+
+module spicore_testbench;
+
+reg clk_i;
+reg rst_i;
+
+reg [4:0]  opcode_i;
+reg [31:0] info_i;
+reg [31:0] dat_i;
+reg [31:0] adr_i;
+
+wire [31:0] dat_o;
+wire dat_o_read;
+wire dat_o_valid;
+wire finish_o;
+
+m25p16_spi_core uut (
+  .clk_i(clk_i), 
+  .rst_i(rst_i), 
+  .opcode_i(opcode_i), 
+  .info_i(info_i), 
+  .dat_i(dat_i), 
+  .dat_o(dat_o), 
+  .dat_o_read(dat_o_read), 
+  .dat_o_valid(dat_o_valid), 
+  .adr_i(adr_i), 
+  .finish_o(finish_o)
+);
+
+initial begin
+  clk_i = 0;
+  forever begin
+    clk_i = !clk_i;
+    #50;
+  end
+end
+
+initial begin
+  clk_i = 0;
+  rst_i = 0;
+  opcode_i = 0;
+  info_i = 0;
+  dat_i = 0;
+  adr_i = 0;
+
+  #100;
+      
+  opcode_i = 2;
+  info_i   = 32'h00000001;
+  dat_i    = 32'hAABBCCDD;
+  adr_i    = 32'h00AABBCC;
+  
+  while( finish_o == 1'b0 )
+    @( posedge clk_i );
+end
+
+endmodule
diff --git a/spiroutine_testbench.v b/spiroutine_testbench.v
new file mode 100644
index 0000000..e62127c
--- /dev/null
+++ b/spiroutine_testbench.v
@@ -0,0 +1,79 @@
+`timescale 1ns / 1ps
+
+module spiroutine_testbench;
+reg [31:0] tx_i;
+reg rst_i;
+reg miso_i;
+reg clk_i;
+reg ss_i;
+reg rd_i;
+reg wr_i;
+reg [7:0] cnt_i;
+
+wire [31:0] rx_o;
+wire mosi_o;
+wire sclk_o;
+wire finish_o;
+wire next_o;
+
+spi_routine uut (
+  .tx_i(tx_i), 
+  .rx_o(rx_o), 
+  .rst_i(rst_i), 
+  .miso_i(miso_i), 
+  .clk_i(clk_i), 
+  .ss_i(ss_i), Q
+  .mosi_o(mosi_o), 
+  .sclk_o(sclk_o), 
+  .rd_i(rd_i), 
+  .wr_i(wr_i), 
+  .cnt_i(cnt_i), 
+  
+  .next_o(next_o),
+  .finish_o(finish_o)
+);
+
+initial begin
+  clk_i = 0;
+  forever begin
+    clk_i = !clk_i;
+    #50;
+  end
+end
+
+initial begin
+  // Initialize Inputs
+  tx_i = 0;
+  rst_i = 0;
+  miso_i = 0;
+  clk_i = 0;
+  ss_i = 0;
+  rd_i = 0;
+  wr_i = 0;
+  cnt_i = 0;
+
+  #100;
+    
+  wr_i = 1;
+  cnt_i = 8;
+  tx_i = 32'hAABBCCDD;
+  ss_i = 1;
+  
+  @( posedge clk_i );
+  
+  wr_i = 0;
+  
+  
+  @( posedge clk_i );
+    while( next_o == 1'b0 )
+      @( posedge clk_i );
+      
+  tx_i = 32'h11223344;
+  
+  @( posedge clk_i );
+    while( finish_o == 1'b0 )
+      @( posedge clk_i );
+  
+  $finish();
+end
+endmodule