diff --git a/verilog/rtl/Flash/FlashBuffer.v b/verilog/rtl/Flash/FlashBuffer.v
index acaae31..29c2171 100644
--- a/verilog/rtl/Flash/FlashBuffer.v
+++ b/verilog/rtl/Flash/FlashBuffer.v
@@ -4,18 +4,22 @@
 		input wire clk,
 		input wire rst,
 
-		// Flash cache interface		
-		input wire flashCache_readEnable,
-		input wire[23:0] flashCache_address,
-		input wire[3:0] flashCache_byteSelect,
-		output wire[31:0] flashCache_dataRead,
-		output wire flashCache_busy,
+		// Peripheral Bus
+		input wire peripheralBus_we,
+		input wire peripheralBus_oe,
+		input wire[23:0] peripheralBus_address,
+		input wire[3:0] peripheralBus_byteSelect,
+		input wire[31:0] peripheralBus_dataWrite,
+		output reg[31:0] peripheralBus_dataRead,
+		output wire peripheralBus_busy,
 
 		// QSPI device
-		output wire[23:0] dataRequest_address,
-		output wire dataRequest_enable,
-		input wire[31:0] dataRequest_data,
-		input wire dataRequest_dataValid,
+		output wire qspi_enable,
+		output wire[23:0] qspi_address,
+		output wire qspi_changeAddress,
+		output reg qspi_requestData,
+		input wire[31:0] qspi_readData,
+		input wire qspi_readDataValid,
 
 		// Flash controller SRAM rw port
 		output wire sram_clk0,
@@ -32,39 +36,149 @@
 		output wire[SRAM_ADDRESS_SIZE-1:0] sram_addr1,
 		input wire[31:0] sram_dout1
 	);
-	
-	//TODO: Flash cache control
-	assign dataRequest_address = flashCache_address;
-	assign dataRequest_enable = 1'b0;
+
+	reg[23:0] cachedAddress;
+	reg[SRAM_ADDRESS_SIZE:0] cachedCount;
+	wire[SRAM_ADDRESS_SIZE:0] nextCachedCount = cachedCount + 1;
+	wire[SRAM_ADDRESS_SIZE:0] cachedCountFinal = { 1'b1, {(SRAM_ADDRESS_SIZE){1'b0}} };
+
+	// Select
+	wire sramEnable = peripheralBus_address[23:SRAM_ADDRESS_SIZE+2] == {(22-SRAM_ADDRESS_SIZE){1'b0}};
+	wire registersEnable = peripheralBus_address[23:12] == 12'h001;
+	wire[11:0] localAddress = peripheralBus_address[11:0];
+
+	// Register
+	// Configuration register 	Default 0x0
+	// b00: enable				Default 0x0
+	wire[31:0] configurationRegisterOutputData;
+	wire configurationRegisterOutputRequest;
+	wire configuration;
+	ConfigurationRegister #(.WIDTH(1), .ADDRESS(12'h000), .DEFAULT(1'b0)) configurationRegister(
+		.clk(clk),
+		.rst(rst),
+		.enable(registersEnable),
+		.peripheralBus_we(peripheralBus_we),
+		.peripheralBus_oe(peripheralBus_oe),
+		.peripheralBus_address(localAddress),
+		.peripheralBus_byteSelect(peripheralBus_byteSelect),
+		.peripheralBus_dataWrite(peripheralBus_dataWrite),
+		.peripheralBus_dataRead(configurationRegisterOutputData),
+		.requestOutput(configurationRegisterOutputRequest),
+		.currentValue(configuration));
+
+	assign qspi_enable = configuration;
+
+	// Base address register 	 Default 0x0
+	reg[23:0] baseAddress;
+	wire[31:0] baseAddressRegisterOutputData;
+	wire baseAddressRegisterOutputRequest;
+	wire baseAddressRegisterBusBusy_nc;
+	wire[23:0] baseAddressRegisterWriteData;
+	wire baseAddressRegisterWriteDataEnable;
+	wire baseAddressRegisterReadDataEnable_nc;
+	DataRegister #(.WIDTH(24), .ADDRESS(12'h004)) baseAddressRegister(
+		.clk(clk),
+		.rst(rst),
+		.enable(registersEnable),
+		.peripheralBus_we(peripheralBus_we),
+		.peripheralBus_oe(peripheralBus_oe),
+		.peripheralBus_busy(baseAddressRegisterBusBusy_nc),
+		.peripheralBus_address(localAddress),
+		.peripheralBus_byteSelect(peripheralBus_byteSelect),
+		.peripheralBus_dataWrite(peripheralBus_dataWrite),
+		.peripheralBus_dataRead(baseAddressRegisterOutputData),
+		.requestOutput(baseAddressRegisterOutputRequest),
+		.writeData(baseAddressRegisterWriteData),
+		.writeData_en(baseAddressRegisterWriteDataEnable),
+		.writeData_busy(1'b0),
+		.readData(baseAddress),
+		.readData_en(baseAddressRegisterReadDataEnable_nc),
+		.readData_busy(1'b0));
+
+	always @(posedge clk) begin
+		if (rst) baseAddress <= 24'b0;
+		else if (baseAddressRegisterWriteDataEnable) baseAddress <= baseAddressRegisterWriteData;
+	end
+
+	// Cached address register
+	wire[31:0] cachedAddressRegisterOutputData;
+	wire cachedAddressRegisterOutputRequest;
+	wire cachedAddressRegisterBusBusy_nc;
+	wire[23:0] cachedAddressRegisterWriteData_nc;
+	wire cachedAddressRegisterWriteDataEnable_nc;
+	wire cachedAddressRegisterReadDataEnable_nc;
+	DataRegister #(.WIDTH(24), .ADDRESS(12'h008)) cachedAddressRegister(
+		.clk(clk),
+		.rst(rst),
+		.enable(registersEnable),
+		.peripheralBus_we(peripheralBus_we),
+		.peripheralBus_oe(peripheralBus_oe),
+		.peripheralBus_busy(cachedAddressRegisterBusBusy_nc),
+		.peripheralBus_address(localAddress),
+		.peripheralBus_byteSelect(peripheralBus_byteSelect),
+		.peripheralBus_dataWrite(peripheralBus_dataWrite),
+		.peripheralBus_dataRead(cachedAddressRegisterOutputData),
+		.requestOutput(cachedAddressRegisterOutputRequest),
+		.writeData(cachedAddressRegisterWriteData_nc),
+		.writeData_en(cachedAddressRegisterWriteDataEnable_nc),
+		.writeData_busy(1'b0),
+		.readData(cachedAddress),
+		.readData_en(cachedAddressRegisterReadDataEnable_nc),
+		.readData_busy(1'b0));
 
 	// Remember that the read data is only valid on the next clock cycle
 	reg flashCacheReadReady = 1'b0;
 	always @(posedge clk) begin
 		if (rst) flashCacheReadReady <= 1'b0;
-		else if (flashCache_readEnable) flashCacheReadReady <= 1'b1;
+		else if (peripheralBus_oe && sramEnable) flashCacheReadReady <= 1'b1;
 		else flashCacheReadReady <= 1'b0;
 	end
 
-	assign flashCache_dataRead = {
-		flashCache_byteSelect[3] && flashCacheReadReady ? sram_dout1[31:24] : 8'h00,
-		flashCache_byteSelect[2] && flashCacheReadReady ? sram_dout1[23:16] : 8'h00,
-		flashCache_byteSelect[1] && flashCacheReadReady ? sram_dout1[15:8]  : 8'h00,
-		flashCache_byteSelect[0] && flashCacheReadReady ? sram_dout1[7:0]   : 8'h00
-	};
+	// Assign peripheral read
+	always @(*) begin
+		case (1'b1)
+			configurationRegisterOutputRequest: peripheralBus_dataRead <= configurationRegisterOutputData;
+			baseAddressRegisterOutputRequest: peripheralBus_dataRead <= baseAddressRegisterOutputData;
+			cachedAddressRegisterOutputRequest: peripheralBus_dataRead <= cachedAddressRegisterOutputData;
+			flashCacheReadReady: peripheralBus_dataRead <= sram_dout1;
+			default: peripheralBus_dataRead <= 32'b0;
+		endcase
+	end
 
-	assign flashCache_busy = flashCache_readEnable && !flashCacheReadReady;
+	assign peripheralBus_busy = peripheralBus_oe && sramEnable && !flashCacheReadReady;
 
+	// QSPI interface
+	always @(posedge clk) begin
+		if (rst) begin
+			cachedAddress <= 32'b0;
+			cachedCount <= {SRAM_ADDRESS_SIZE{1'b0}};
+			qspi_requestData <= 1'b0;
+		end	else if (baseAddressRegisterWriteDataEnable) begin
+			cachedAddress <= baseAddressRegisterWriteData[23:2];
+			cachedCount <= {SRAM_ADDRESS_SIZE{1'b0}};
+			qspi_requestData <= 1'b1;
+		end else if (qspi_requestData && qspi_readDataValid) begin
+			cachedAddress <= cachedAddress + 4;
+			cachedCount <= nextCachedCount;
+			qspi_requestData <= nextCachedCount != cachedCountFinal;
+		end
+	end
+
+	assign qspi_address = baseAddressRegisterWriteData;
+	assign qspi_changeAddress = baseAddressRegisterWriteDataEnable;
+
+	// Assign sram port
+	// Read/write port
 	assign sram_clk0 = clk;
-	assign sram_csb0 = 1'b1;	// Active low chip enable
-	assign sram_web0 = 1'b1;	// Active low write enable (probably keep as always write)
+	assign sram_csb0 = !(qspi_requestData && qspi_readDataValid);	// Active low chip enable
+	assign sram_web0 = 1'b0;	// Active low write enable (probably keep as always write)
 	assign sram_wmask0 = 4'b1111;
-	assign sram_addr0 = {SRAM_ADDRESS_SIZE{1'b0}};
-	assign sram_din0 = 32'b0;
+	assign sram_addr0 = cachedAddress[SRAM_ADDRESS_SIZE+1:2];
+	assign sram_din0 = qspi_readData;
 
 	// Read port
-	wire validAddress = flashCache_address[23:SRAM_ADDRESS_SIZE+1] == 'b0;
 	assign sram_clk1 = clk;
-	assign sram_csb1 = !(validAddress && flashCache_readEnable);
-	assign sram_addr1 = flashCache_address[SRAM_ADDRESS_SIZE+1:2];
+	assign sram_csb1 = !(sramEnable && peripheralBus_oe);
+	assign sram_addr1 = peripheralBus_address[SRAM_ADDRESS_SIZE+1:2];
 
 endmodule
\ No newline at end of file
diff --git a/verilog/rtl/Flash/Flash_top.v b/verilog/rtl/Flash/Flash_top.v
index 16370a2..d5f31a2 100644
--- a/verilog/rtl/Flash/Flash_top.v
+++ b/verilog/rtl/Flash/Flash_top.v
@@ -52,13 +52,18 @@
 	localparam STATE_READ_CONTINUOUS = 2'b11;
 	
 	// Wishbone interface
-	wire flashCache_readEnable;
-	wire[23:0] flashCache_address;
-	wire[3:0] flashCache_byteSelect;
-	wire[31:0] flashCache_dataRead;
-	wire flashCache_busy;
-
-	WBFlashInterface wbFlashInterface(
+	wire peripheralBus_we;
+	wire peripheralBus_oe;
+	wire peripheralBus_busy;
+	wire[23:0] peripheralBus_address;
+	wire[3:0] peripheralBus_byteSelect;
+	wire[31:0] peripheralBus_dataRead;
+	wire[31:0] peripheralBus_dataWrite;
+	WBPeripheralBusInterface wbPeripheralBusInterface(
+	`ifdef USE_POWER_PINS
+		.vccd1(vccd1),	// User area 1 1.8V power
+		.vssd1(vssd1),	// User area 1 digital ground
+	`endif
 		.wb_clk_i(wb_clk_i),
 		.wb_rst_i(wb_rst_i),
 		.wb_stb_i(wb_stb_i),
@@ -71,30 +76,38 @@
 		.wb_stall_o(wb_stall_o),
 		.wb_error_o(wb_error_o),
 		.wb_data_o(wb_data_o),
-		.flashCache_readEnable(flashCache_readEnable),
-		.flashCache_address(flashCache_address),
-		.flashCache_byteSelect(flashCache_byteSelect),
-		.flashCache_dataRead(flashCache_dataRead),
-		.flashCache_busy(flashCache_busy));
+		.peripheralBus_we(peripheralBus_we),
+		.peripheralBus_oe(peripheralBus_oe),
+		.peripheralBus_address(peripheralBus_address),
+		.peripheralBus_byteSelect(peripheralBus_byteSelect),
+		.peripheralBus_dataWrite(peripheralBus_dataWrite),
+		.peripheralBus_dataRead(peripheralBus_dataRead),
+		.peripheralBus_busy(peripheralBus_busy));
 
 	// Flash cache
-	wire[23:0] dataRequest_address;
-	wire dataRequest_enable;
-	wire[31:0] dataRequest_data;
-	wire dataRequest_dataValid;
+	wire qspi_enable;
+	wire[23:0] qspi_address;
+	wire qspi_changeAddress;
+	wire qspi_requestData;
+	wire[31:0] qspi_readData;
+	wire qspi_readDataValid;
 
-	FlashBuffer flashBuffer(
+	FlashBuffer #(.SRAM_ADDRESS_SIZE(SRAM_ADDRESS_SIZE)) flashBuffer(
 		.clk(wb_clk_i),
 		.rst(wb_rst_i),
-		.flashCache_readEnable(flashCache_readEnable),
-		.flashCache_address(flashCache_address),
-		.flashCache_byteSelect(flashCache_byteSelect),
-		.flashCache_dataRead(flashCache_dataRead),
-		.flashCache_busy(flashCache_busy),
-		.dataRequest_address(dataRequest_address),
-		.dataRequest_enable(dataRequest_enable),
-		.dataRequest_data(dataRequest_data),
-		.dataRequest_dataValid(dataRequest_dataValid),
+		.peripheralBus_we(peripheralBus_we),
+		.peripheralBus_oe(peripheralBus_oe),
+		.peripheralBus_address(peripheralBus_address),
+		.peripheralBus_byteSelect(peripheralBus_byteSelect),
+		.peripheralBus_dataWrite(peripheralBus_dataWrite),
+		.peripheralBus_dataRead(peripheralBus_dataRead),
+		.peripheralBus_busy(peripheralBus_busy),
+		.qspi_enable(qspi_enable),
+		.qspi_address(qspi_address),
+		.qspi_changeAddress(qspi_changeAddress),
+		.qspi_requestData(qspi_requestData),
+		.qspi_readData(qspi_readData),
+		.qspi_readDataValid(qspi_readDataValid),
 		.sram_clk0(sram_clk0),
 		.sram_csb0(sram_csb0),
 		.sram_web0(sram_web0),
@@ -108,13 +121,15 @@
 		.sram_dout1(sram_dout1));
 
 	// QSPI controller
-	QSPIDevice #(.CLOCK_WIDTH(8)) qspiDevice (
+	QSPIDevice qspiDevice (
 		.clk(wb_clk_i),
 		.rst(wb_rst_i),
-		.dataRequest_address(dataRequest_address),
-		.dataRequest_enable(dataRequest_enable),
-		.dataRequest_data(dataRequest_data),
-		.dataRequest_dataValid(dataRequest_dataValid),
+		.qspi_enable(qspi_enable),
+		.qspi_address(qspi_address),
+		.qspi_changeAddress(qspi_changeAddress),
+		.qspi_requestData(qspi_requestData),
+		.qspi_readData(qspi_readData),
+		.qspi_readDataValid(qspi_readDataValid),
 		.flash_csb(flash_csb),
 		.flash_sck(flash_sck),
 		.flash_io0_we(flash_io0_we),
@@ -124,101 +139,4 @@
 		.flash_io1_write(flash_io1_write),
 		.flash_io1_read(flash_io1_read));
 
-	// Flash controller
-	// wire doneTransferByte = 1'b0;
-	// reg[1:0] state = STATE_STARTUP;
-	
-	// reg[7:0] dataBuffer [0:3];
-	// reg[1:0] bufferPointer = 2'h0;
-	// wire[2:0] nextBufferPointer = bufferPointer + 1;
-
-	// reg[31:0] currentAddress = 32'b0;
-	// wire[31:0] nextAddress = currentAddress + 1;
-
-	// reg[31:0] outputData;
-
-	// always @(*) begin
-	// 	case (bufferPointer)
-	// 		2'h0: outputData <= { dataBuffer[3], dataBuffer[2], dataBuffer[1], dataBuffer[0] };
-	// 		2'h1: outputData <= { dataBuffer[2], dataBuffer[1], dataBuffer[0], dataBuffer[3] };
-	// 		2'h2: outputData <= { dataBuffer[1], dataBuffer[0], dataBuffer[3], dataBuffer[2] };
-	// 		2'h3: outputData <= { dataBuffer[0], dataBuffer[3], dataBuffer[2], dataBuffer[1] };
-	// 	endcase
-	// end
-
-	// // Parallel interface
-	// wire writeEnable;
-	// wire readEnable;
-	// wire[7:0] writeData;
-	// wire[7:0] readData;
-	// wire busy;
-
-	
-		
-	// always @(posedge clk) begin
-	// 	if (rst) begin
-	// 		state <= STATE_STARTUP;
-	// 		bufferPointer <= 3'h0;
-	// 		dataBuffer[0] <= 8'hFF;
-	// 		dataBuffer[1] <= 8'hAB;
-	// 		dataBuffer[2] <= 8'h03;
-	// 		dataBuffer[3] <= 8'h00;
-	// 	end else begin
-	// 		case (state)
-	// 			STATE_STARTUP: begin
-	// 				if (doneTransferByte) begin
-	// 					if (bufferPointer == 3'h1) begin 
-	// 						state <= STATE_CHANGE_ADDRESS;
-	// 						dataBuffer[0] <= 8'h03;
-	// 						dataBuffer[1] <= 8'h00;
-	// 						dataBuffer[2] <= 8'h00;
-	// 						dataBuffer[3] <= 8'h00;
-	// 						bufferPointer <= 3'h0;
-	// 					end	else begin
-	// 						bufferPointer <= nextBufferPointer;
-	// 					end
-	// 				end
-	// 			end
-
-	// 			STATE_WRITE_COMMAND: begin
-	// 				if (doneTransferByte) begin
-	// 					dataBuffer[bufferPointer] <= readData;
-	// 					bufferPointer <= nextBufferPointer;
-	// 					currentAddress <= nextAddress;
-	// 				end
-	// 			end
-
-	// 			STATE_CHANGE_ADDRESS: begin
-	// 				if (doneTransferByte) begin
-	// 					if (bufferPointer == 3'h1) begin 
-	// 						state <= STATE_READ_CONTINUOUS;
-	// 						dataBuffer[0] <= 8'h00;
-	// 						dataBuffer[1] <= 8'h00;
-	// 						dataBuffer[2] <= 8'h00;
-	// 						dataBuffer[3] <= 8'h00;
-	// 						bufferPointer <= 3'h0;
-	// 					end	else begin
-	// 						bufferPointer <= nextBufferPointer;
-	// 					end
-	// 				end
-	// 			end
-
-	// 			STATE_READ_CONTINUOUS: begin
-					
-	// 			end
-
-	// 			default:  begin
-	// 				state <= STATE_STARTUP;
-	// 				bufferPointer <= 3'h0;
-	// 				dataBuffer[0] <= 8'hFF;
-	// 				dataBuffer[1] <= 8'hAB;
-	// 				dataBuffer[2] <= 8'h03;
-	// 				dataBuffer[3] <= 8'h00;
-	// 			end
-	// 		endcase
-	// 	end
-	// end
-
-	// assign writeData = dataBuffer[bufferPointer];
-
 endmodule
\ No newline at end of file
diff --git a/verilog/rtl/Flash/QSPIDevice.v b/verilog/rtl/Flash/QSPIDevice.v
index f1e3d7f..c59e366 100644
--- a/verilog/rtl/Flash/QSPIDevice.v
+++ b/verilog/rtl/Flash/QSPIDevice.v
@@ -1,18 +1,18 @@
-module QSPIDevice #(
-		parameter CLOCK_WIDTH = 8
-	)(
+module QSPIDevice (
 		input wire clk,
 		input wire rst,
 
 		// Cache interface
-		input wire[23:0] dataRequest_address,
-		input wire dataRequest_enable,
-		output wire[31:0] dataRequest_data,
-		output wire dataRequest_dataValid,
+		input wire qspi_enable,
+		input wire[23:0] qspi_address,
+		input wire qspi_changeAddress,
+		input wire qspi_requestData,
+		output wire[31:0] qspi_readData,
+		output reg qspi_readDataValid,
 
 		// QSPI interface
 		output wire flash_csb,
-		output wire flash_sck,
+		output reg flash_sck,
 		output wire flash_io0_we,
 		output wire flash_io0_write,
 		input wire flash_io0_read, 	// Unused
@@ -21,218 +21,129 @@
 		input wire flash_io1_read
 	);
 	
-	// Have this running at the system clock rate
-	// This will probably be 40MHz
-	assign flash_sck = clk;
+	localparam STATE_IDLE = 2'h0;
+	localparam STATE_SETUP = 2'h1;
+	localparam STATE_SHIFT = 2'h2;
+	localparam STATE_END = 2'h3;
 
-	assign flash_csb = 1'b0;
+	localparam RESET_NONE = 2'h0;
+	localparam RESET_START = 2'h1;
+	localparam RESET_WAKE = 2'h2;
+
+	// Assign these as constants to be in a default spi mode
 	assign flash_io0_we = 1'b1;
-	assign flash_io0_write = 1'b0;
 	assign flash_io1_we = 1'b0;
 	assign flash_io1_write = 1'b0;
-	//assign in = flash_io1_read;
 
-	assign dataRequest_data = 32'b0;
-	assign dataRequest_dataValid = 1'b1;
+	// State control
+	reg[1:0] state = STATE_IDLE;
+	wire deviceBusy = state != STATE_IDLE;
+	reg[1:0] resetState = RESET_NONE;
+	wire resetDevice = resetState != RESET_NONE;
 
-	// localparam CLOCK_BITS = $clog2(CLOCK_WIDTH);
-
-	// localparam STATE_IDLE  = 2'b00;
-	// localparam STATE_SETUP = 2'b01;
-	// localparam STATE_SHIFT = 2'b10;
-	// localparam STATE_END   = 2'b11;
-
-	// // Device select
-	// wire[11:0] localAddress;
-	// wire deviceEnable;
-	// DeviceSelect #(.ID(ID)) select(
-	// 	.peripheralEnable(peripheralEnable),
-	// 	.peripheralBus_address(peripheralBus_address),
-	// 	.localAddress(localAddress),
-	// 	.deviceEnable(deviceEnable));
-
-	// // Register
-	// // Configuration register 	Default 0x064
-	// // b00-b02: clockScale 		Default 0x4
-	// // b03-04: spiMode 			Default 0x0
-	// // b05: msbFirst 			Default 0x1
-	// // b06: useCS 				Default 0x1
-	// // b07: activeHighCS		Default 0x0
-	// // b08: enable				Default 0x0
-	// wire[31:0] configurationRegisterOutputData;
-	// wire configurationRegisterOutputRequest;
-	// wire[8:0] configuration;
-	// ConfigurationRegister #(.WIDTH(9), .ADDRESS(12'h000), .DEFAULT(9'h064)) configurationRegister(
-	// 	.clk(clk),
-	// 	.rst(rst),
-	// 	.enable(deviceEnable),
-	// 	.peripheralBus_we(peripheralBus_we),
-	// 	.peripheralBus_oe(peripheralBus_oe),
-	// 	.peripheralBus_address(localAddress),
-	// 	.peripheralBus_byteSelect(peripheralBus_byteSelect),
-	// 	.peripheralBus_dataWrite(peripheralBus_dataWrite),
-	// 	.peripheralBus_dataRead(configurationRegisterOutputData),
-	// 	.requestOutput(configurationRegisterOutputRequest),
-	// 	.currentValue(configuration));
-
-	// wire[2:0] clockScale = configuration[2:0];
-	// wire[1:0] spiMode = configuration[4:3];
-	// wire msbFirst = configuration[5];
-	// wire useCS = configuration[6];
-	// wire activeHighCS = configuration[7];
-	// assign spi_en = configuration[8];
-	// wire spiClockPolarity = spiMode[1];
-	// wire spiSampleMode = spiMode[0];
-
-	// // Input and Output register
-	// wire[31:0] dataRegisterOutputData;
-	// wire dataRegisterOutputRequest;
-	// wire[7:0] readData;
-	// wire[7:0] writeData;	
-	// wire writeData_en;
-	// wire dataRegisterBusBusy_nc;
-	// wire dataRegisterReadDataEnable_nc;
-	// DataRegister #(.WIDTH(8), .ADDRESS(12'h004)) dataRegister(
-	// 	.clk(clk),
-	// 	.rst(rst),
-	// 	.enable(deviceEnable),
-	// 	.peripheralBus_we(peripheralBus_we),
-	// 	.peripheralBus_oe(peripheralBus_oe),
-	// 	.peripheralBus_busy(dataRegisterBusBusy_nc),
-	// 	.peripheralBus_address(localAddress),
-	// 	.peripheralBus_byteSelect(peripheralBus_byteSelect),
-	// 	.peripheralBus_dataWrite(peripheralBus_dataWrite),
-	// 	.peripheralBus_dataRead(dataRegisterOutputData),
-	// 	.requestOutput(dataRegisterOutputRequest),
-	// 	.writeData(writeData),
-	// 	.writeData_en(writeData_en),
-	// 	.writeData_busy(1'b0),
-	// 	.readData(readData),
-	// 	.readData_en(dataRegisterReadDataEnable_nc),
-	// 	.readData_busy(1'b0));
-
-	// // State control
-	// reg[1:0] state = STATE_IDLE;
-	// wire busy = state != STATE_IDLE;
+	reg outputClock = 1'b0;	
+	reg[4:0] bitCounter = 5'b0;
+	wire[4:0] nextBitCounter = bitCounter + 1;
 	
-	// reg[2:0] bitCounter = 3'b0;
-	// wire[2:0] nextBitCounter = bitCounter + 1;
+	wire shiftInEnable  = outputClock && deviceBusy;
+	wire shiftOutEnable = !outputClock && deviceBusy;
 
-	// reg[CLOCK_WIDTH-1:0] clockCounter = {CLOCK_WIDTH{1'b0}};
-	// wire nextClockCounter = clockCounter + 1;
-	// wire[CLOCK_WIDTH-1:0] clockScaleHalfMask = {(CLOCK_WIDTH-1){1'b0}, 1'b1} << clockScale;
-	// wire[CLOCK_WIDTH-1:0] clockScaleMask 	 = { clockScaleMask[CLOCK_WIDTH-2:0], 1'b0 };
-	// wire spiHalfClock = clockCounter == (clockScaleHalfMask - 1);//|(clockCounter & clockScaleHalfMask);
-	// wire spiClock 	  = clockCounter == (clockScaleMask - 1);
+	reg[31:0] registerLoadData;
+	wire serialOut;
+	wire[31:0] flashData;
+	ShiftRegister #(.WIDTH(32)) register (
+		.clk(clk),
+		.rst(rst),
+		.loadEnable((!deviceBusy && qspi_changeAddress) || ((state == STATE_SETUP) && resetDevice)),
+		.shiftInEnable(shiftInEnable),
+		.shiftOutEnable(shiftOutEnable),
+		.msbFirst(1'b1),
+		.parallelIn(registerLoadData),
+		.parallelOut(flashData),
+		.serialIn(flash_io1_read),
+		.serialOut(serialOut));
 
-	// reg spiClockRise = 1'b0;
-	// reg spiClockFall = 1'b0;
+	// Reorder bytes
+	assign qspi_readData = { flashData[7:0], flashData[15:8], flashData[23:16], flashData[31:24] };
 
-	// wire shiftInEnable  = spiSampleMode ? spiClockFall : spiClockRise;
-	// wire shiftOutEnable = spiSampleMode ? spiClockRise : spiClockFall;
+	always @(*) begin
+		case (1'b1)
+			resetState == RESET_START: registerLoadData <= { 8'hFF, 8'h00, 8'h00, 8'h00 };
+			resetState == RESET_WAKE: registerLoadData <= { 8'hAB, 8'h00, 8'h00, 8'h00 };
+			qspi_changeAddress: registerLoadData <= { 8'h03, qspi_address };
+			default: registerLoadData <= 32'b0;
+		endcase
+	end
 
-	// reg loadEnable;
-	// ShiftRegister #(.WIDTH(8)) register (
-	// 	.clk(clk),
-	// 	.rst(rst),
-	// 	.loadEnable(loadEnable),
-	// 	.shiftInEnable(shiftInEnable),
-	// 	.shiftOutEnable(shiftOutEnable),
-	// 	.msbFirst(msbFirst),
-	// 	.parallelIn(writeData),
-	// 	.parallelOut(readData),
-	// 	.serialIn(spi_miso),
-	// 	.serialOut(spi_mosi));
+	always @(posedge clk) begin
+		if (rst) begin
+			state <= STATE_IDLE;
+			outputClock <= 1'b0;
+			bitCounter <= 5'b0;
+			resetState <= RESET_START;
+			qspi_readDataValid <= 1'b0;
+		end else begin
+			case (state)
+				STATE_IDLE: begin
+					outputClock <= 1'b0;
+					bitCounter <= 5'b0;
 
-	// always @(posedge clk) begin
-	// 	if (rst) begin
-	// 		state <= STATE_IDLE;
-	// 		bitCounter <= 3'b0;
-	// 		clockCounter <= {CLOCK_WIDTH{1'b0}};
-	// 		loadEnable <= 1'b0;
-	// 		spiClockRise <= 1'b0;
-	// 		spiClockFall <= 1'b0;
-	// 	end else begin
-	// 		case (state)
-	// 			STATE_IDLE: begin
-	// 				bitCounter <= 3'b0;
-	// 				clockCounter <= {CLOCK_WIDTH{1'b0}};
-	// 				loadEnable <= 1'b0;
+					if (qspi_enable) begin
+						if (resetDevice || qspi_changeAddress) state <= STATE_SETUP;
+					end
+				end
 
-	// 				if (writeData_en && peripheralBus_byteSelect[0]) begin 
-	// 					state <= STATE_SETUP;
-	// 					loadEnable <= 1'b1;
-	// 				end
-	// 			end
+				STATE_SETUP: begin
+					state <= STATE_SHIFT;
+					bitCounter <= 5'b0;
+					outputClock <= 1'b1;
+					qspi_readDataValid <= 1'b0;		
+				end
 
-	// 			STATE_SETUP: begin
-	// 				loadEnable <= 1'b0;
+				STATE_SHIFT: begin
+					if (!outputClock) begin
+						if ((resetDevice && bitCounter == 5'h07) || (bitCounter == 5'h1F)) begin
+							state <= STATE_END;
+							qspi_readDataValid <= 1'b1;
+						end	else begin
+							bitCounter <= nextBitCounter;
+						end
+					end
 
-	// 				if (spiHalfClock) begin
-	// 					clockCounter <= {CLOCK_WIDTH{1'b0}};
-	// 					bitCounter <= 1'b0;
-	// 					state <= STATE_SHIFT;
-	// 				end else begin
-	// 					clockCounter <= nextClockCounter;
-	// 				end
-	// 			end
+					outputClock <= !outputClock;
 
-	// 			STATE_SHIFT: begin
-	// 				if (spiClock) begin
-	// 					if (spiClockPolarity) begin
-	// 						spiClockRise <= 1'b0;
-	// 						spiClockFall <= 1'b1;
-	// 					end else begin
-	// 						spiClockRise <= 1'b1;
-	// 						spiClockFall <= 1'b0;
-	// 					end
+					if (qspi_changeAddress) state <= STATE_IDLE;
+				end
 
-	// 					clockCounter <= {CLOCK_WIDTH{1'b0}};
-	// 					if (bitCounter == 3'h7) state <= STATE_END;
-	// 					else bitCounter <= nextBitCounter;
-	// 				end else if (spiHalfClock) begin
-	// 					if (spiClockPolarity) begin
-	// 						spiClockRise <= 1'b1;
-	// 						spiClockFall <= 1'b0;
-	// 					end else begin
-	// 						spiClockRise <= 1'b0;
-	// 						spiClockFall <= 1'b1;
-	// 					end
-						
-	// 				end else begin
-	// 					spiClockRise <= 1'b0;
-	// 					spiClockFall <= 1'b0;
-	// 					clockCounter <= nextClockCounter;
-	// 				end
-	// 			end
+				STATE_END: begin
+					if (qspi_requestData) state <= STATE_SETUP;
+					else state <= STATE_IDLE;
 
-	// 			STATE_END: begin
-	// 				spiClockRise <= 1'b0;
-	// 				spiClockFall <= 1'b0;
+					outputClock <= 1'b0;
+					qspi_readDataValid <= 1'b0;
 					
-	// 				if (spiClock) state <= STATE_IDLE;
-	// 				else clockCounter <= nextClockCounter;
-	// 			end
+					if (resetState == RESET_START) resetState <= RESET_WAKE;
+					else resetState <= RESET_NONE;
+				end
 
-	// 			default: begin
-	// 				state <= STATE_IDLE;
-	// 				bitCounter <= 3'b0;
-	// 				clockCounter <= {CLOCK_WIDTH{1'b0}};
-	// 				loadEnable <= 1'b0;
-	// 				spiClockRise <= 1'b0;
-	// 				spiClockFall <= 1'b0;
-	// 			end
-	// 		endcase
-	// 	end
-	// end
+				default: begin
+					state <= STATE_IDLE;
+					bitCounter <= 5'b0;
+					outputClock <= 1'b0;
+					qspi_readDataValid <= 1'b0;
+					resetState <= RESET_START;
+				end
+			endcase
+		end
+	end
 
-	// assign requestOutput = configurationRegisterOutputRequest || dataRegisterOutputRequest;
-	// assign peripheralBus_dataRead = configurationRegisterOutputRequest ? configurationRegisterOutputData :
-	// 							    dataRegisterOutputRequest 		   ? dataRegisterOutputData :
-	// 												   					 32'b0;																			
-	// assign peripheralBus_busy = busy;
+	// Buffer the spi clock by one cycle so that it lines up with when data is sampled
+	always @(posedge clk) begin
+		if (rst) flash_sck <= 1'b0;
+		else flash_sck <= outputClock;
+	end
 
-	// assign spi_clk = spiClockPolarity ? !(spiClock && busy) : spiClock && busy;
-	// assign spi_cs = useCS ? (activeHighCS ? busy : !busy) : 1'b0;
+	assign flash_io0_write = serialOut & deviceBusy;
+	assign flash_csb = !deviceBusy;
 
 endmodule
\ No newline at end of file
diff --git a/verilog/rtl/Flash/WBFlashInterface.v b/verilog/rtl/Flash/WBFlashInterface.v
deleted file mode 100644
index 60ea02d..0000000
--- a/verilog/rtl/Flash/WBFlashInterface.v
+++ /dev/null
@@ -1,110 +0,0 @@
-module WBFlashInterface (
-		// Wishbone Slave ports
-		input wire wb_clk_i,
-		input wire wb_rst_i,
-		input wire wb_stb_i,
-		input wire wb_cyc_i,
-		input wire wb_we_i,
-		input wire[3:0] wb_sel_i,
-		input wire[31:0] wb_data_i,
-		input wire[23:0] wb_adr_i,
-		output wire wb_ack_o,
-		output wire wb_stall_o,
-		output wire wb_error_o,
-		output wire[31:0] wb_data_o,
-
-		// Flash cache interface
-		output wire flashCache_readEnable,
-		output wire[23:0] flashCache_address,
-		output wire[3:0] flashCache_byteSelect,
-		input wire[31:0] flashCache_dataRead,
-		input wire flashCache_busy
-	);
-
-	localparam STATE_IDLE  		  = 2'h0;
-	localparam STATE_WRITE_SINGLE = 2'h1;
-	localparam STATE_READ_SINGLE  = 2'h2;
-	localparam STATE_FINISH 	  = 2'h3;
-	
-	reg[1:0] state = STATE_IDLE;
-	reg[23:0] currentAddress;
-	reg[3:0] currentByteSelect;
-	reg[31:0] currentDataIn;
-
-	reg stall = 1'b0;
-	reg acknowledge = 1'b0;
-	reg[31:0] dataRead_buffered;
-
-	always @(posedge wb_clk_i) begin
-		if (wb_rst_i) begin
-			state <= STATE_IDLE;
-			stall <= 1'b0;
-			acknowledge <= 1'b0;
-			dataRead_buffered <= ~32'b0;
-		end else begin
-			case (state)
-				STATE_IDLE: begin
-					stall <= 1'b0;
-					acknowledge <= 1'b0;
-					dataRead_buffered <= ~32'b0;
-
-					if (wb_cyc_i) begin
-						if (wb_stb_i) begin
-							currentAddress <= wb_adr_i;
-							currentByteSelect <= wb_sel_i;
-							currentDataIn <= wb_data_i;
-							stall <= 1'b1;
-
-							if (wb_we_i) begin
-								state <= STATE_WRITE_SINGLE;
-							end else begin
-								state <= STATE_READ_SINGLE;
-							end
-						end
-					end
-				end
-
-				STATE_WRITE_SINGLE: begin
-					// Not allowed to write, so just acknowledge so the bus doesn't hang
-					state <= STATE_FINISH;
-					acknowledge <= 1'b1;
-				end
-
-				STATE_READ_SINGLE: begin
-					if (!flashCache_busy) begin
-						state <= STATE_FINISH;
-						acknowledge <= 1'b1;
-						dataRead_buffered <= flashCache_dataRead;
-					end
-				end
-
-				STATE_FINISH: begin
-					state <= STATE_IDLE;
-					stall <= 1'b0;
-					acknowledge <= 1'b0;
-					dataRead_buffered <= ~32'b0;
-				end
-
-				default: begin
-					state <= STATE_IDLE;
-					stall <= 1'b0;
-					acknowledge <= 1'b0;
-				end
-			endcase
-		end
-	end
-
-	// Connect wishbone bus signals
-	assign wb_ack_o = acknowledge;
-	assign wb_stall_o = stall;
-	assign wb_error_o = 1'b0;
-
-	// Connect peripheral bus signals
-	assign flashCache_readEnable = state == STATE_READ_SINGLE;
-
-	assign flashCache_address = state != STATE_IDLE ? currentAddress : 24'b0;
-	assign flashCache_byteSelect = state != STATE_IDLE ? currentByteSelect : 4'b0;
-
-	assign wb_data_o = dataRead_buffered;
-	
-endmodule
\ No newline at end of file