NvmeStreamMux: Improvements.
authorTerry Barnaby <terry.barnaby@beam.beam.ltd.uk>
Tue, 2 Jun 2020 08:05:52 +0000 (09:05 +0100)
committerTerry Barnaby <terry.barnaby@beam.beam.ltd.uk>
Tue, 2 Jun 2020 08:05:52 +0000 (09:05 +0100)
Large modifications to test software.

src/NvmeStreamMux.vhd
test/BeamLibBasic.cpp
test/BeamLibBasic.h
test/Makefile
test/NvmeAccess.cpp
test/NvmeAccess.h
test/test_nvme.cpp

index 722e13c57a4f815b323b9cace77c29653a059c87..d39e3b1fe0f89827ba02bf7e611b363d6152f75c 100644 (file)
@@ -70,7 +70,8 @@ signal nvmeNumber     : std_logic;
 type MuxStateType      is (MUX_STATE_START, MUX_STATE_SENDPACKET0, MUX_STATE_SENDPACKET1);
 signal muxState                : MuxStateType := MUX_STATE_START;
 signal muxReply                : std_logic;
-signal nvme1Stream     : std_logic;
+signal nvmeStream      : std_logic;
+signal nvmeStream0Last : std_logic;
 signal nvme1StreamData : std_logic_vector(127 downto 0);
 
 begin
@@ -132,41 +133,42 @@ begin
        end process;
        
        
-       -- Multiplex streams. Sets the Nvme number to 1 in the Nvme1 reply streams in appropriate location for request and reply packets
-       nvme1Stream <= '1' when(((muxState = MUX_STATE_START) and (nvme1In.valid = '1')) or (muxState = MUX_STATE_SENDPACKET1)) else '0';
+       -- Multiplex streams. Sets the Nvme number based on if streams have valid status and toggling between streams to give a fair share
+       nvmeStream <= '0' when(((muxState = MUX_STATE_START) and (nvme0In.valid = '1') and (nvmeStream0Last = '0')) or (muxState = MUX_STATE_SENDPACKET0)) else '1';
 
        nvme1StreamData <= nvme1In.data(127 downto 81) & '1' & nvme1In.data(79 downto 0) when((muxState = MUX_STATE_START) and (nvme1In.data(95) = '1'))
                else nvme1In.data(127 downto 32) & x"1" & nvme1In.data(27 downto 0) when((muxState = MUX_STATE_START) and (nvme1In.data(95) = '0'))
                else nvme1In.data;
        
-       hostOut.valid <= nvme0In.valid when(nvme1Stream = '0') else nvme1In.valid;
-       hostOut.last <= nvme0In.last when(nvme1Stream = '0') else nvme1In.last;
-       hostOut.keep <= nvme0In.keep when(nvme1Stream = '0') else nvme1In.keep;
-       hostOut.data <= nvme0In.data when(nvme1Stream = '0')  else nvme1StreamData;
+       hostOut.valid <= nvme0In.valid when(nvmeStream = '0') else nvme1In.valid;
+       hostOut.last <= nvme0In.last when(nvmeStream = '0') else nvme1In.last;
+       hostOut.keep <= nvme0In.keep when(nvmeStream = '0') else nvme1In.keep;
+       hostOut.data <= nvme0In.data when(nvmeStream = '0')  else nvme1StreamData;
 
-       nvme0In.ready <= hostOut.ready when(nvme1Stream = '0') else '0';
-       nvme1In.ready <= hostOut.ready when(nvme1Stream = '1') else '0';
+       nvme0In.ready <= hostOut.ready when(nvmeStream = '0') else '0';
+       nvme1In.ready <= hostOut.ready when(nvmeStream = '1') else '0';
 
        process(clk)
        begin
                if(rising_edge(clk)) then
                        if(reset = '1') then
-                               muxState <= MUX_STATE_START;
+                               nvmeStream0Last <= '0';
+                               muxState        <= MUX_STATE_START;
                        else
                                case(muxState) is
                                when MUX_STATE_START =>
-                                       if((nvme0In.valid = '1') and (hostOut.ready = '1')) then
-                                               if(nvme0In.last = '1') then
-                                                       muxState <= MUX_STATE_START;
-                                               else
+                                       if((nvme0In.valid = '1') and (nvme0In.ready = '1')) then
+                                               nvmeStream0Last <= '1';
+                                               if(nvme0In.last = '0') then
                                                        muxState <= MUX_STATE_SENDPACKET0;
                                                end if;
-                                       elsif((nvme1In.valid = '1') and (hostOut.ready = '1')) then
-                                               if(nvme1In.last = '1') then
-                                                       muxState <= MUX_STATE_START;
-                                               else
+                                       elsif((nvme1In.valid = '1') and (nvme1In.ready = '1')) then
+                                               nvmeStream0Last <= '0';
+                                               if(nvme1In.last = '0') then
                                                        muxState <= MUX_STATE_SENDPACKET1;
                                                end if;
+                                       elsif((nvme0In.valid = '1') and (nvme1In.valid = '0')) then
+                                               nvmeStream0Last <= '0';
                                        end if;
 
                                when MUX_STATE_SENDPACKET0 =>
index f5549db7268a17f5c7acb4a7981866840cf274c3..7c3a38e438f32987d3fab7a8100856ecedf4775a 100644 (file)
@@ -32,6 +32,8 @@
 
 #include <BeamLibBasic.h>
 #include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
 #include <time.h>
 #include <sys/time.h>
 
@@ -84,6 +86,111 @@ int BSemaphore::getValue() const {
        return v;
 }
 
+// Simple Byte Fifo implementation
+
+// The BFifoBytes functions
+BFifoBytes::BFifoBytes(BUInt size){
+       osize = size;
+       odata = new char [osize];
+       owritePos = 0;
+       oreadPos = 0;
+}
+
+BFifoBytes::~BFifoBytes(){
+       delete [] odata;
+       odata = 0;
+       osize = 0;
+       owritePos = 0;
+       oreadPos = 0;
+}
+
+void BFifoBytes::clear(){
+       owritePos = 0;
+       oreadPos = 0;
+}
+
+BUInt BFifoBytes::size(){
+       return osize;
+}
+
+int BFifoBytes::resize(BUInt size){
+       int     err = 0;
+       
+       delete [] odata;
+       osize = size;
+       odata = new char [osize];
+       owritePos = 0;
+       oreadPos = 0;
+       
+       return err;
+}
+
+BUInt BFifoBytes::writeAvailable(){
+       BUInt   readPos = oreadPos;
+
+       if(readPos <= owritePos)
+               return osize - owritePos + readPos - 1;
+       else
+               return (readPos - owritePos - 1);
+}
+
+int BFifoBytes::write(const void* data, BUInt num){
+       int     err = 0;
+       char*   d = (char*)data;
+       BUInt   nt;
+
+       while(num){
+               nt = num;
+               if(nt > (osize - owritePos))
+                       nt = (osize - owritePos);
+               
+               memcpy(&odata[owritePos], d, nt);
+
+               if((owritePos + nt) == osize)
+                       owritePos = 0;
+               else
+                       owritePos += nt;
+
+               d += nt;
+               num -= nt;
+       }
+               
+       return err;
+}
+
+BUInt BFifoBytes::readAvailable(){
+       BUInt           writePos = owritePos;
+       
+       if(oreadPos <= writePos)
+               return writePos - oreadPos;
+       else
+               return osize - oreadPos + writePos;
+}
+
+int BFifoBytes::read(void* data, BUInt num){
+       int     err = 0;
+       char*   d = (char*)data;
+       BUInt   nt;
+
+       while(num){
+               nt = num;
+               if(nt > (osize - oreadPos))
+                       nt = (osize - oreadPos);
+               
+               memcpy(d, &odata[oreadPos], nt);
+
+               if((oreadPos + nt) == osize)
+                       oreadPos = 0;
+               else
+                       oreadPos += nt;
+
+               d += nt;
+               num -= nt;
+       }
+
+       return err;
+}
+
 
 
 void tprintf(const char* fmt, ...){
@@ -110,7 +217,8 @@ void bhd8(void* data, BUInt32 n){
                if((i & 0xF) == 0xF)
                        printf("\n");
        }
-       printf("\n");
+       if(n % 16)
+               printf("\n");
 }
 
 void bhd32(void* data, BUInt32 n){
@@ -122,7 +230,8 @@ void bhd32(void* data, BUInt32 n){
                if((i & 0x7) == 0x7)
                        printf("\n");
        }
-       printf("\n");
+       if(n % 8)
+               printf("\n");
 }
 
 void bhd32a(void* data, BUInt32 n){
@@ -140,7 +249,7 @@ void bhd32a(void* data, BUInt32 n){
                a += 4;
        }
        
-       if(n % 16)
+       if(n % 8)
                printf("\n");
 }
 
index ccf4fcaafc075f1c51f23eee8778102e6d4ed227..8f693499fc19e3c440e8615633d6381d3b35fe04 100644 (file)
@@ -94,6 +94,31 @@ private:
        sem_t                   osema;
 };
 
+// Simple Byte Fifo
+class BFifoBytes {
+public:
+                       BFifoBytes(BUInt size);
+                       ~BFifoBytes();
+
+       void            clear();
+
+       BUInt           size();                                         ///< Returns fifo size
+       int             resize(BUInt size);                             ///< Resize FIFO, clears it as well
+
+       BUInt           writeAvailable();                               ///< How many items that can be written
+       int             write(const void* data, BUInt num);             ///< Write a set of items. Can only write a maximum of writeAvailableChunk() to save going beyond end of FIFO buffer
+
+       BUInt           readAvailable();                                ///< How many items are available to read
+       int             read(void* data, BUInt num);                    ///< Read a set of items
+
+protected:
+       BUInt           osize;                                          ///< The size of the FIFO
+       char*           odata;                                          ///< FIFO memory buffer
+       volatile BUInt  owritePos;                                      ///< The write pointer
+       volatile BUInt  oreadPos;                                       ///< The read pointer
+};
+
+
 void tprintf(const char* fmt, ...);
 void bhd8(void* data, BUInt32 n);
 void bhd32(void* data,BUInt32 n);
index b669f17d5c83ce13ed295faea6a23f54efc1a89b..ca2bc175e9532c312ec58e280a4fb8014dc1f4e4 100644 (file)
@@ -7,6 +7,8 @@
 PROG           = test_nvme
 OBJS           = test_nvme.o NvmeAccess.o BeamLibBasic.o
 
+#CXXFLAGS      += -g
+CXXFLAGS       += -O
 CXXFLAGS       += -I.
 CXXFLAGS       += -MMD
 LDLIBS         += -lpthread
@@ -31,4 +33,4 @@ driver_load:
 ${PROG}: ${OBJS}
 
 # Dependancies
--include $(OBJS:.o=.d)
\ No newline at end of file
+-include $(OBJS:.o=.d)
index caf026fd07580e848ebeebdc84193b9c3fa323be..7be54933f66a0c036b34ecd0666be23bc5b423e7 100644 (file)
@@ -175,24 +175,11 @@ void NvmeAccess::setNvme(BUInt n){
                onvmeRegbase = 0x000;
 }
 
-#ifdef ZAP
-void NvmeAccess::reset(){
-       BUInt32 data;
-
-       dl1printf("NvmeAccess::reset\n");
-       writeNvmeStorageReg(4, 0x00000001);
-
-       data = 1;
-       while(data & 3){
-               data = readNvmeStorageReg(8);
-               usleep(1000);
-       }
-       usleep(100000);
-
-       data = 0x06;
-       pcieWrite(10, 4, 1, &data);                     ///< Set PCIe config command for memory accesses
+BUInt NvmeAccess::getNvme(){
+       return onvmeNum;
 }
-#else
+
+#if LDEBUG1
 void NvmeAccess::reset(){
        BUInt32 data;
        double  ts, te;
@@ -227,9 +214,28 @@ void NvmeAccess::reset(){
                usleep(100000);
                printf("Last status was: %8.8x\n", data);
        }
+       usleep(100000);
+}
+#else
+void NvmeAccess::reset(){
+       BUInt32 data;
+
+       dl1printf("NvmeAccess::reset\n");
+       writeNvmeStorageReg(4, 0x00000001);
+
+       if(UseFpgaConfigure){
+               data = 1;
+               while(data & 3){
+                       data = readNvmeStorageReg(8);
+                       usleep(1000);
+               }
+       }
        else {
-               data = 0x06;
-               pcieWrite(10, 4, 1, &data);                     ///< Set PCIe config command for memory accesses
+               data = 1;
+               while(data & 1){
+                       data = readNvmeStorageReg(8);
+                       usleep(1000);
+               }
        }
        usleep(100000);
 }
@@ -359,6 +365,8 @@ int NvmeAccess::nvmeProcess(){
                if(request.request == 0){
                        // PCIe Read requests
                        dl3printf("NvmeAccess::nvmeProcess: Read memory: address: %8.8x nWords: %d\n", request.address, request.numWords);
+                       printf("NvmeAccess::nvmeProcess: Read memory: address: %8.8x nWords: %d\n", request.address, request.numWords);
+
                        if((request.address & 0x00FF0000) == 0x00000000){
                                data = oqueueAdminMem;
                        }
@@ -451,10 +459,12 @@ int NvmeAccess::nvmeProcess(){
 
                                memcpy(&odataBlockMem[(request.address & 0x0000FFFF) / 4], request.data, request.numWords * 4);
                        }
-                       else if((request.address & 0x00FF0000) == 0x00F00000){
-                               printf("NvmeAccess::nvmeProcess: Write: address: %8.8x nWords: %d\n", (request.address & 0x0FFFFFFF), nWords);
-                               memcpy(&odataBlockMem[(request.address & 0x0000FFFF) / 4], request.data, request.numWords * 4);
-                               bhd32(odataBlockMem, request.numWords);
+                       else if((request.address & 0x00F00000) == 0x00F00000){
+                               dl3printf("NvmeAccess::nvmeProcess: Write: address: %8.8x nWords: %d\n", (request.address & 0x0FFFFFFF), nWords);
+
+                               //memcpy(&odataBlockMem[(request.address & 0x00000FFF) / 4], request.data, request.numWords * 4);
+                               //dl3hd32(odataBlockMem, request.numWords);
+                               nvmeDataPacket(request);
                        }
                        else {
                                printf("NvmeAccess::nvmeProcess: Write data: unknown address: 0x%8.8x\n", request.address);
@@ -473,6 +483,9 @@ int NvmeAccess::nvmeProcess(){
        return 0;
 }
 
+void NvmeAccess::nvmeDataPacket(NvmeRequestPacket& packet){
+}
+
 BUInt32 NvmeAccess::readNvmeStorageReg(BUInt32 address){
        return oregs[onvmeRegbase/4 + address/4];
 }
@@ -541,10 +554,6 @@ int NvmeAccess::pcieWrite(BUInt8 request, BUInt32 address, BUInt32 num, BUInt32*
                if(opacketReply.error)
                        return opacketReply.error;
        }
-       else {
-               // Not sure why this is needed ?
-               //usleep(1000);
-       }
        
        return 0;
 }
index 7214fbbc765ab888e3c6ce221058011649421b6d..f5a616d45209d22535826bf648c690a6bc387a98 100644 (file)
 #include <sys/ioctl.h>
 #include <bfpga_driver/bfpga.h>
 
-const Bool     UseQueueEngine = 1;                     ///< Use the FPGA queue engine implementation
+const Bool     UseFpgaConfigure = 0;                   ///< Expect the NvmeStorage module to have configured the Nvme's
 const Bool     UseConfigEngine = 0;                    ///< Use the FPGA configuration engine
+const Bool     UseQueueEngine = 1;                     ///< Use the FPGA queue engine implementation
 const BUInt    PcieMaxPayloadSize = 32;                ///< The Pcie maximim packet payload in 32bit DWords
+const BUInt    BlockSize = 4096;                       ///< The NvmeStorage block size in bytes
 
 const BUInt    RegIdent                = 0x000;        ///< The ident and version
 const BUInt    RegControl              = 0x004;        ///< The control register
@@ -138,6 +140,7 @@ public:
        void            close();
 
        void            setNvme(BUInt n);
+       BUInt           getNvme();
        void            reset();
 
        // Send a queued request to the NVMe
@@ -145,6 +148,7 @@ public:
        
        // NVMe process received requests thread
        int             nvmeProcess();
+       virtual void    nvmeDataPacket(NvmeRequestPacket& packet);                      ///< Called when read data packet received
        
        // NvmeStorage units register access
        BUInt32         readNvmeStorageReg(BUInt32 address);
index 6f302f7abadf158bcd1cebb800accc7c5dadb8ba..6100b88590fa8e1ce1bbedb93719cdec2892ba6d 100644 (file)
@@ -47,6 +47,7 @@
 #include <NvmeAccess.h>
 #include <stdio.h>
 #include <getopt.h>
+#include <stdarg.h>
 
 #define VERSION                "0.0.1"
 
@@ -57,9 +58,22 @@ public:
                        ~Control();
 
        int             init();                                 ///< Initialise
+       void            setStartBlock(BUInt32 startBlock);      ///< Set the starting block number
+       void            setNumBlocks(BUInt32 numBlocks);        ///< Set the number of blocks to operate on
+       void            setFilename(const char* filename);      ///< Set the file name for read data
 
+       int             nvmeInit();                             ///< Reset and configure Nvme's for operation
+       int             nvmeConfigure();                        ///< Configure single Nvme for operation
+       void            nvmeDataPacket(NvmeRequestPacket& packet);      ///< Called when read data packet receiver
+
+       // Normal test functions
+       int             nvmeProcess();                          ///< Process FPGA datastream writing to Nvme
+       int             nvmeTrim();                             ///< Trim blocks on Nvme
+       int             nvmeWrite();                            ///< Write blocks to Nvme
+       int             nvmeRead();                             ///< Read blocks from Nvme
+
+       // Basic/Raw test functions
        int             test1();                                ///< Run test1
-       int             configureNvme();                        ///< Configure Nvme for operation
        int             test2();                                ///< Run test2
        int             test3();                                ///< Run test3
        int             test4();                                ///< Run test4
@@ -69,19 +83,36 @@ public:
        int             test8();                                ///< Run test8
        int             test9();                                ///< Run test9
        int             test10();                               ///< Run test10
-
        int             test_misc();                            ///< Collection of misc tests
 
+       // Support functions
+       void            uprintf(const char* fmt, ...);          ///< User verbose printf
+       int             validateBlock(BUInt32 blockNum, void* data);    ///< Validate a data block
+       void            dumpDataBlock(void* data, Bool full);   ///< Print out a data blocks contents
        void            dumpNvmeRegisters();                    ///< Dump the Nvme registers to stdout
 
 public:
        // Params
-       Bool            overbose;
-       NvmeAccess      onvmeAccess;
+       Bool            overbose;                               ///< Verbose operation
+       Bool            ovalidate;                              ///< Validate data
+       BUInt32         ostartBlock;                            ///< The starting block number
+       BUInt32         onumBlocks;                             ///< The number of blocks
+       const char*     ofilename;                              ///< Output file name
+       
+       BFifoBytes      ofifo0;                                 ///< Fifo for Nvme0 read data
+       BFifoBytes      ofifo1;                                 ///< Fifo for Nvme1 read data
+       BUInt32         oblockNum;                              ///< The output block number
+       BUInt8          odataBlock[BlockSize];                  ///< Data block's from NVme's
+       BSemaphore      oreadComplete;                          ///< The read process is complete
 };
 
-Control::Control(){
+Control::Control() : ofifo0(1024*1024), ofifo1(1024*1024){
        overbose = 0;
+       ovalidate = 1;
+       ostartBlock = 0;
+       onumBlocks = 1;
+       ofilename = 0;
+       oblockNum = 0;
 }
 
 Control::~Control(){
@@ -91,49 +122,73 @@ int Control::init(){
        return NvmeAccess::init();
 }
 
-int Control::test1(){
-       BUInt32 data[8];
+void Control::setStartBlock(BUInt32 startBlock){
+       ostartBlock = startBlock;
+}
 
-       printf("Test1: Simple PCIe command register read, write and read.\n");
+void Control::setNumBlocks(BUInt32 numBlocks){
+       onumBlocks = numBlocks;
+}
 
-       printf("Configure PCIe for memory accesses\n");
-       pcieRead(8, 4, 1, data);
-       dl1printf("Commandreg: %8.8x\n", data[0]);
+void Control::setFilename(const char* filename){
+       ofilename = filename;
+}
 
-       data[0] |= 6;
-       pcieWrite(10, 4, 1, data);
+int Control::nvmeInit(){
+       int     e;
+       
+       printf("Initialise Nvme's for operation\n");
+       
+       // Perform reset
+       reset();
 
-       pcieRead(8, 4, 1, data);
-       dl1printf("Commandreg: %8.8x\n", data[0]);
+       if(!UseFpgaConfigure){
+               if(onvmeNum == 2){
+                       setNvme(0);
+                       if(e = nvmeConfigure())
+                               return e;
 
-       printf("Complete\n");
+                       setNvme(1);
+                       if(e = nvmeConfigure())
+                               return e;
 
-       return 0;
+                       setNvme(2);
+               }
+               else {
+                       e = nvmeConfigure();
+               }
+       }
+       
+       return e;
 }
 
-int Control::configureNvme(){
+int Control::nvmeConfigure(){
        int     e;
        BUInt32 data;
        BUInt32 cmd0;
 
-       printf("Configure Nvme for operation\n");
+       uprintf("nvmeConfigure: Configure Nvme %u for operation\n", onvmeNum);
        
-       // Perform reset
-       reset();
-
 #ifdef ZAP
        dumpNvmeRegisters();
        return 0;
 #endif
 
-#ifndef ZAP
        if(UseConfigEngine){    
-               printf("Start configuration\n");
+               uprintf("Start configuration\n");
                writeNvmeStorageReg(4, 0x00000002);
-               usleep(100000);
-               printf("Waited 100ms: Status: %8.8x\n", readNvmeStorageReg(RegStatus));
+
+               data = 2;
+               while(data & 2){
+                       data = readNvmeStorageReg(8);
+                       usleep(1000);
+               }
+               uprintf("Configuration complete: Status: %8.8x\n", readNvmeStorageReg(RegStatus));
        }
        else {
+               data = 0x06;
+               pcieWrite(10, 4, 1, &data);                     ///< Set PCIe config command for memory accesses
+
 #ifdef ZAP
                // Setup Max payload, hardcoded for Seagate Nvme
                pcieRead(8, 4, 1, &data);
@@ -169,143 +224,94 @@ int Control::configureNvme(){
                // Stop controller
                if(e = writeNvmeReg32(0x14, 0x00460000)){
                        printf("Error: %d\n", e);
-                       return 1;
+                       return e;
                }
                usleep(10000);
 
                // Setup Nvme registers
                // Disable interrupts
                if(e = writeNvmeReg32(0x0C, 0xFFFFFFFF)){
-                       printf("Error: %d\n", e);
-                       return 1;
+                       return e;
                }
 
                // Admin queue lengths
                if(e = writeNvmeReg32(0x24, ((oqueueNum - 1) << 16) | (oqueueNum - 1))){
-                       printf("Error: %d\n", e);
-                       return 1;
+                       return e;
                }
 
                if(UseQueueEngine){
                        // Admin request queue base address
                        if(e = writeNvmeReg64(0x28, 0x02000000)){
-                               printf("Error: %d\n", e);
-                               return 1;
+                               return e;
                        }
 
                        // Admin reply queue base address
                        //if(e = writeNvmeReg64(0x30, 0x01100000)){             // Get replies sent directly to host
                        if(e = writeNvmeReg64(0x30, 0x02100000)){               // Get replies sent via QueueEngine
-                               printf("Error: %d\n", e);
-                               return 1;
+                               return e;
                        }
                }
                else {
                        // Admin request queue base address
                        if(e = writeNvmeReg64(0x28, 0x01000000)){
-                               printf("Error: %d\n", e);
-                               return 1;
+                               return e;
                        }
 
                        // Admin reply queue base address
                        if(e = writeNvmeReg64(0x30, 0x01100000)){
-                               printf("Error: %d\n", e);
-                               return 1;
+                               return e;
                        }
                }
 
                // Start controller
                if(e = writeNvmeReg32(0x14, 0x00460001)){
-                       printf("Error: %d\n", e);
-                       return 1;
+                       return e;
                }
+               
+               // Wait for Nvme to start
                usleep(100000);
 
                //dumpNvmeRegisters();
 
                cmd0 = ((oqueueNum - 1) << 16);
 
-#ifdef ZAP
-               // Test the queue engine
-               printf("Create/delete IO queue 1 for replies repeatidly\n");
-
-               if(UseQueueEngine){
-                       for(int c = 0; c < 10; c++){
-                               printf("Do: %d\n", c);
-
-                               nvmeRequest(0, 0, 0x05, 0x02110000, cmd0 | 1, 0x00000001);
-                               sleep(1);
-
-                               nvmeRequest(0, 0, 0x04, 0x02110000, cmd0 | 1, 0x00000001);
-                               sleep(1);
-                       }
-               }
-               else {
-                       for(int c = 0; c < 10; c++){
-                               printf("Do: %d\n", c);
-
-                               nvmeRequest(0, 0, 0x05, 0x00110000, cmd0 | 1, 0x00000001);
-                               sleep(1);
-
-                               nvmeRequest(0, 0, 0x04, 0x00110000, cmd0 | 1, 0x00000001);
-                               sleep(1);
-                       }
-               }
-               return 0;
-#endif
-
                if(UseQueueEngine){
                        // Create an IO queue
-                       if(overbose)
-                               printf("Create IO queue 1 for replies\n");
-
+                       uprintf("Create IO queue 1 for replies\n");
                        nvmeRequest(1, 0, 0x05, 0x02110000, cmd0 | 1, 0x00000001);
 
                        // Create an IO queue
-                       if(overbose)
-                               printf("Create IO queue 1 for requests\n");
-
+                       uprintf("Create IO queue 1 for requests\n");
                        nvmeRequest(1, 0, 0x01, 0x02010000, cmd0 | 1, 0x00010001);
 
                        // Create an IO queue
-                       if(overbose)
-                               printf("Create IO queue 2 for replies\n");
-
+                       uprintf("Create IO queue 2 for replies\n");
                        nvmeRequest(1, 0, 0x05, 0x02120000, cmd0 | 2, 0x00000001);
 
                        // Create an IO queue
-                       if(overbose)
-                               printf("Create IO queue 1 for requests\n");
-
+                       uprintf("Create IO queue 2 for requests\n");
                        nvmeRequest(1, 0, 0x01, 0x02020000, cmd0 | 2, 0x00020001);
                }
                else {
                        // Create an IO queue
-                       if(overbose)
-                               printf("Create IO queue 1 for replies\n");
-
+                       uprintf("Create IO queue 1 for replies\n");
                        nvmeRequest(1, 0, 0x05, 0x01110000, cmd0 | 1, 0x00000001);
 
                        // Create an IO queue
-                       if(overbose)
-                               printf("Create IO queue 1 for requests\n");
-
+                       uprintf("Create IO queue 1 for requests\n");
                        nvmeRequest(1, 0, 0x01, 0x01010000, cmd0 | 1, 0x00010001);
 
                        // Create an IO queue
-                       if(overbose)
-                               printf("Create IO queue 2 for replies\n");
-
+                       uprintf("Create IO queue 2 for replies\n");
                        nvmeRequest(1, 0, 0x05, 0x01120000, cmd0 | 2, 0x00000001);
 
                        // Create an IO queue
-                       if(overbose)
-                               printf("Create IO queue 2 for requests\n");
-
+                       uprintf("Create IO queue 2 for requests\n");
                        nvmeRequest(1, 0, 0x01, 0x01020000, cmd0 | 2, 0x00020001);
                }
        }
-#endif
+
+       // Make sure all is settled
        usleep(100000);
 
        //dumpNvmeRegisters();
@@ -313,11 +319,214 @@ int Control::configureNvme(){
        return 0;
 }
 
+
+
+
+void Control::nvmeDataPacket(NvmeRequestPacket& packet){
+       //printf("Control::nvmeDataPacket: Address: %x\n", packet.address);
+       //bhd32(packet.data, packet.numWords);
+
+       // This assumes the PcieWrites are in order
+       if(packet.address & 0xF0000000){
+               // Nvme 1
+               ofifo1.write(packet.data, packet.numWords * 4);
+       }
+       else {
+               // Nvme 0
+               ofifo0.write(packet.data, packet.numWords * 4);
+       }
+
+       // Output data blocks from FIFO's       
+       while((ofifo0.readAvailable() >= BlockSize) && (ofifo1.readAvailable() >= BlockSize)){
+               ofifo0.read(odataBlock, BlockSize);
+               if(overbose){
+                       printf("Block: %u\n", oblockNum);
+                       dumpDataBlock(odataBlock, 0);
+               }
+               if(ovalidate){
+                       if(validateBlock(oblockNum, odataBlock)){
+                               printf("Error in block: %u startAddress(0x%8.8x)\n", oblockNum, (oblockNum * BlockSize / 4));
+                               dumpDataBlock(odataBlock, 1);
+                               exit(1);
+                       }
+               }
+               
+               oblockNum++;
+
+               ofifo1.read(odataBlock, BlockSize);
+               if(overbose){
+                       printf("Block: %u\n", oblockNum);
+                       dumpDataBlock(odataBlock, 0);
+               }
+               if(ovalidate){
+                       if(validateBlock(oblockNum, odataBlock)){
+                               printf("Error in block: %u startAddress(0x%8.8x)\n", oblockNum, (oblockNum * BlockSize / 4));
+                               dumpDataBlock(odataBlock, 1);
+                               exit(1);
+                       }
+               }
+
+               oblockNum++;
+       }
+       
+       if(oblockNum >= (ostartBlock + onumBlocks))
+               oreadComplete.set();
+}
+
+int Control::nvmeProcess(){
+       int     e;
+       BUInt32 n;
+       BUInt32 t;
+       double  r;
+       double  ts;
+       
+       printf("nvmeProcess: Write FPGA data stream to Nvme devices\n");
+
+       // Initialise Nvme devices
+       if(e = nvmeInit())
+               return e;
+
+       //dumpRegs();
+       
+       // Set number of blocks to write
+       writeNvmeStorageReg(RegDataChunkStart, ostartBlock);
+       writeNvmeStorageReg(RegDataChunkSize, onumBlocks);
+       //dumpRegs();
+       
+       // Start off NvmeWrite engine
+       uprintf("Start NvmeWrite engine\n");
+       writeNvmeStorageReg(4, 0x00000004);
+
+       ts = getTime();
+       n = 0;
+       while(n != onumBlocks){
+               n = readNvmeStorageReg(RegWriteNumBlocks);
+               uprintf("NvmeWrite: numBlocks: %u\n", n);
+               usleep(100000);
+       }
+
+       printf("Time was: %f\n", getTime() - ts);
+       printf("Stats\n");
+       dumpRegs(0);
+       dumpRegs(1);
+
+       n = readNvmeStorageReg(RegWriteNumBlocks);
+       t = readNvmeStorageReg(RegWriteTime);
+       r = ((double(BlockSize) * n) / (1e-6 * t));
+       printf("NvmeWrite: rate:      %f MBytes/s\n", r / (1024 * 1024));
+
+       return 0;
+}
+
+int Control::nvmeRead(){
+       int     e;
+       BUInt32 block = 0;
+       BUInt32 numBlocks = 8;
+       double  r;
+       double  ts;
+       double  te;
+       
+       printf("NvmeRead: nvme: %u startBlock: %u numBlocks: %u\n",onvmeNum, ostartBlock, onumBlocks);
+       
+       if(e = nvmeInit())
+               return e;
+
+       oblockNum = ostartBlock;
+       memset(odataBlock, 0x0, sizeof(odataBlock));
+
+       if(onvmeNum == 2){
+               writeNvmeStorageReg(RegReadBlock, ostartBlock / 2);
+               writeNvmeStorageReg(RegReadNumBlocks, onumBlocks / 2);
+       }
+       else {
+               writeNvmeStorageReg(RegReadBlock, ostartBlock);
+               writeNvmeStorageReg(RegReadNumBlocks, onumBlocks);
+       }
+       
+       if(overbose)
+               dumpRegs();
+       
+       // Start off NvmeRead engine
+       uprintf("Start NvmeRead engine\n");
+       ts = getTime();
+       writeNvmeStorageReg(RegReadControl, 0x00000001);
+
+       if(overbose){
+               setNvme(0);
+               dumpRegs();
+               setNvme(1);
+               dumpRegs();
+       }
+
+       // Wait for complete
+       oreadComplete.wait();
+       te = getTime();
+       
+       printf("Time: %f\n", te - ts);
+
+       r = ((double(BlockSize) * onumBlocks) / (te - ts));
+       printf("NvmeRead: rate:      %f MBytes/s\n", r / (1024 * 1024));
+       
+       printf("Complete\n"); fflush(stdout);
+
+       return 0;
+}
+
+int Control::nvmeWrite(){
+       return 0;
+}
+
+int Control::nvmeTrim(){
+       int     e;
+       BUInt32 block;
+       BUInt   trimBlocks = 32768;
+
+       printf("NvmeTrim: nvme: %u startBlock: %u numBlocks: %u\n",onvmeNum, ostartBlock, onumBlocks);
+       
+       if(e = nvmeInit())
+               return e;
+
+       for(block = 0; block < onumBlocks; block += (trimBlocks/8)){
+               if(onvmeNum == 2){
+                       setNvme(0);
+                       nvmeRequest(1, 1, 0x08, 0x00000000, block * 8, 0x00000000, (1 << 25) | trimBlocks-1);   // Perform trim of 32k 512 Byte blocks
+                       setNvme(1);
+                       nvmeRequest(1, 1, 0x08, 0x00000000, block * 8, 0x00000000, (1 << 25) | trimBlocks-1);   // Perform trim of 32k 512 Byte blocks
+               }
+               else {
+                       nvmeRequest(1, 1, 0x08, 0x00000000, block * 8, 0x00000000, (1 << 25) | trimBlocks-1);   // Perform trim of 32k 512 Byte blocks
+               }
+       }
+
+       return 0;
+}
+
+
+int Control::test1(){
+       BUInt32 data[8];
+
+       printf("Test1: Simple PCIe command register read, write and read.\n");
+
+       printf("Configure PCIe for memory accesses\n");
+       pcieRead(8, 4, 1, data);
+       dl1printf("Commandreg: %8.8x\n", data[0]);
+
+       data[0] |= 6;
+       pcieWrite(10, 4, 1, data);
+
+       pcieRead(8, 4, 1, data);
+       dl1printf("Commandreg: %8.8x\n", data[0]);
+
+       printf("Complete\n");
+
+       return 0;
+}
+
 int Control::test2(){
        int     e;
        
        printf("Test2: Configure Nvme\n");
-       if(e = configureNvme())
+       if(e = nvmeInit())
                return e;
 
        //dumpNvmeRegisters();
@@ -330,7 +539,7 @@ int Control::test3(){
        
        printf("Test3: Get info from Nvme\n");
 
-       if(e = configureNvme())
+       if(e = nvmeInit())
                return e;
 
        printf("Get info\n");
@@ -350,7 +559,7 @@ int Control::test4(){
        printf("Test4: Read blocks\n");
        //onvmeNum = 2;
        
-       if(e = configureNvme())
+       if(e = nvmeInit())
                return e;
 
        printf("Perform block read\n");
@@ -408,7 +617,7 @@ int Control::test5(){
        
        printf("Test5: Write blocks\n");
        
-       if(e = configureNvme())
+       if(e = nvmeInit())
                return e;
 
        srand(time(0));
@@ -439,11 +648,11 @@ int Control::test6(){
        printf("Test6: Enable FPGA write blocks\n");
 
        setNvme(0);
-       if(e = configureNvme())
+       if(e = nvmeInit())
                return e;
 
        setNvme(1);
-       if(e = configureNvme())
+       if(e = nvmeInit())
                return e;
 
        setNvme(2);
@@ -535,7 +744,7 @@ int Control::test7(){
        
        printf("Test7: Validate 4k blocks\n");
        
-       if(e = configureNvme())
+       if(e = nvmeInit())
                return e;
 
        v = 0;
@@ -565,7 +774,7 @@ int Control::test8(){
 
        printf("Test8: Trim Nvme\n");
        
-       if(e = configureNvme())
+       if(e = nvmeInit())
                return e;
 
        for(block = 0; block < numBlocks; block += (maxBlocks/8)){
@@ -631,12 +840,12 @@ int Control::test10(){
        
        printf("Test10: Read blocks using NvmeRead functionality\n");
 
-       if(e = configureNvme())
+       if(e = nvmeInit())
                return e;
 
        //dumpRegs();
        
-       // Set number of blocks to write
+       // Set number of blocks to read
        writeNvmeStorageReg(RegReadBlock, 0);
        writeNvmeStorageReg(RegReadNumBlocks, numBlocks);
        dumpRegs();
@@ -664,7 +873,7 @@ int Control::test_misc(){
 
        printf("Test_misc: Collection of misc tests\n");
        
-       if(e = configureNvme())
+       if(e = nvmeInit())
                return e;
 
        printf("Get info\n");
@@ -695,6 +904,43 @@ int Control::test_misc(){
        return 0;
 }
 
+void Control::uprintf(const char* fmt, ...){
+       va_list         args;
+       
+       if(overbose){
+               va_start(args, fmt);
+               
+               vprintf(fmt, args);
+       }
+}
+
+int Control::validateBlock(BUInt32 blockNum, void* data){
+       BUInt32*        d = (BUInt32*)data;
+       BUInt           w;
+       
+       for(w = 0; w < BlockSize / 4; w++){
+               if(d[w] != ((blockNum * BlockSize / 4) + w)){
+                       printf("Validate Error: Block: %u Position: %u 0x%8.8x !- 0x%8.8x\n", blockNum, w, d[w], ((blockNum * BlockSize / 4) + w));
+                       return 1;
+               }
+       }
+       
+       return 0;
+}
+
+void Control::dumpDataBlock(void* data, Bool full){
+       char*   d = (char*)data;
+       
+       if(full){
+               bhd32(data, BlockSize/4);
+       }
+       else {
+               bhd32(data, 8);
+               printf("...\n");
+               bhd32(&d[BlockSize - (8*4)], 8);
+       }
+}
+
 void Control::dumpNvmeRegisters(){
        int     e;
        BUInt   a;
@@ -710,24 +956,30 @@ void Control::dumpNvmeRegisters(){
        }
 }
 
-
-
 void usage(void) {
        fprintf(stderr, "test_nvme: Version: %s\n", VERSION);
        fprintf(stderr, "Usage: test_nvme [options] <testname>\n");
        fprintf(stderr, "This program provides the ability perform access tests to an Nvme device on a FPGA development board\n");
        fprintf(stderr, " -help,-h              - Help on command line parameters\n");
        fprintf(stderr, " -v                    - Verbose\n");
+       fprintf(stderr, " -no-validate          - Disable data validation\n");
        fprintf(stderr, " -l                    - List tests\n");
-       fprintf(stderr, " -n <nvmeNum>          - Operate on: 0: Nvme0, 1: Nvme1, 2: Both Nvme's\n");
+       fprintf(stderr, " -d <nvmeNum>          - Nvme to operate on: 0: Nvme0, 1: Nvme1, 2: Both Nvme's (default)\n");
+       fprintf(stderr, " -s <block>            - The starting 4k block number (default is 0)\n");
+       fprintf(stderr, " -n <num>              - The number of blocks to read/write or trim (default is 1)\n");
+       fprintf(stderr, " -o <filename>         - The filename for output data.\n");
 }
 
 static struct option options[] = {
                { "h",                  0, NULL, 0 },
                { "help",               0, NULL, 0 },
                { "v",                  0, NULL, 0 },
+               { "no-validate",        0, NULL, 0 },
                { "l",                  0, NULL, 0 },
+               { "d",                  1, NULL, 0 },
+               { "s",                  1, NULL, 0 },
                { "n",                  1, NULL, 0 },
+               { "o",                  1, NULL, 0 },
                { 0,0,0,0 }
 };
 int main(int argc, char** argv){
@@ -748,12 +1000,24 @@ int main(int argc, char** argv){
                else if(!strcmp(s, "v")){
                        control.overbose = 1;
                }
+               else if(!strcmp(s, "no-validate")){
+                       control.ovalidate = 0;
+               }
                else if(!strcmp(s, "l")){
                        listTests = 1;
                }
-               else if(!strcmp(s, "n")){
+               else if(!strcmp(s, "d")){
                        control.setNvme(atoi(optarg));
                }
+               else if(!strcmp(s, "s")){
+                       control.setStartBlock(atoi(optarg));
+               }
+               else if(!strcmp(s, "n")){
+                       control.setNumBlocks(atoi(optarg));
+               }
+               else if(!strcmp(s, "o")){
+                       control.setFilename(optarg);
+               }
                else {
                        fprintf(stderr, "Error: No option: %s\n", s);
                        usage();
@@ -761,13 +1025,23 @@ int main(int argc, char** argv){
                }
        }
        
+       if(control.getNvme() == 2){
+               if(control.ostartBlock & 1){
+                       fprintf(stderr, "Needs an even start block number when two Nvme's are being accessed\n");
+                       return 1;
+               }
+               if(control.onumBlocks & 1){
+                       fprintf(stderr, "Needs an even number of blocks when two Nvme's are being accessed\n");
+                       return 1;
+               }
+       }
+       
        if(listTests){
-               printf("test1: Simple PCIe command register read, write and read.\n");
-               printf("test2: Configure Nvme\n");
-               printf("test3: Get info from Nvme\n");
-               printf("test4: Read block\n");
-               printf("test5: Write block\n");
-               printf("test_misc: Collection of misc tests\n");
+               printf("process: Perform data input from FPGA TestData source into Nvme's.\n");
+               printf("read: Read data from Vvme's\n");
+               printf("write: Write data to Nvme's\n");
+               printf("trim: Trim/deallocate blocks on Nvme's\n");
+               printf("test*: Collection of misc programmed tests. See source code.\n");
        }
        else {
                if((argc - optind) != 1){
@@ -781,7 +1055,21 @@ int main(int argc, char** argv){
                        return err;
                }
 
-               if(!strcmp(test, "test1")){
+               if(!strcmp(test, "process")){
+                       err = control.nvmeProcess();
+               }
+               else if(!strcmp(test, "read")){
+                       err = control.nvmeRead();
+               }
+               else if(!strcmp(test, "write")){
+                       err = control.nvmeWrite();
+               }
+               else if(!strcmp(test, "trim")){
+                       err = control.nvmeTrim();
+               }
+               
+               // Basic programed tests
+               else if(!strcmp(test, "test1")){
                        err = control.test1();
                }
                else if(!strcmp(test, "test2")){