Added Nvme trim/deallocate functionality.
[DuneNvme.git] / test / NvmeAccess.cpp
1 /*******************************************************************************
2  *      NvmeAccess.cpp  Provides access to an Nvme storage device on FpgaFabric
3  *      T.Barnaby,      Beam Ltd,       2020-04-10
4  *******************************************************************************
5  */
6 /**
7  * @class       NvmeAccess
8  * @author      Terry Barnaby <terry.barnaby@beam.ltd.uk>
9  * @date        2020-04-10
10  * @version     0.0.1
11  *
12  * @brief
13  * This is a simple class that provides access to an Nvme storage device on FpgaFabric.
14  *
15  * @details
16  * This requires an Nvme device on a KCU105 with the DuneNvmeStorageTest bit file running.
17  * The system allows an NVMe situtated on the Xilinx KCU105 to be accessed and experimented with. It implements the following:
18  *  - Configuration of the NVMe PCIe configuration space registers.
19  *  - Accessing the NVMe registers.
20  *  - Configuration of the NVMe's registers.
21  *  - Sending Admin commands to the NVMe via the admin request/completion shared memory queues. This includes configuration commands.
22  *  - Sending of read and write IO commands to the NVMe via IO request/completion shared memory queues.
23  *
24  * There is access to the memory mappend NvmeStorage registers and there is one bi-directional DMA stream used for communication.
25  * The send and receive DMA streams are multiplexed between requests from the host and replies from the Nvme and also
26  * requests from the Nvme and replies from the host.
27  * The packets sent have a 128bit multiplexing stream number headerand are then encapsulated in the Xilinx PCIe DMA IP's headers.
28  *
29  * The class accesses the FPGA system over the hosts PCIe bus using the Beam bfpga Linux driver. This interfaces with the Xilinx PCIe DMA IP.
30  * The class uses a thread to respond to Nvme requests.
31  *
32  * @copyright GNU GPL License
33  * Copyright (c) Beam Ltd, All rights reserved. <br>
34  * This code is free software: you can redistribute it and/or modify
35  * it under the terms of the GNU General Public License as published by
36  * the Free Software Foundation, either version 3 of the License, or
37  * (at your option) any later version.
38  * This program is distributed in the hope that it will be useful,
39  * but WITHOUT ANY WARRANTY; without even the implied warranty of
40  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
41  * GNU General Public License for more details. <br>
42  * You should have received a copy of the GNU General Public License
43  * along with this code. If not, see <https://www.gnu.org/licenses/>.
44  */
45 #define LDEBUG1         0               // High level debug
46 #define LDEBUG2         0               // Debug host to NVMe queued requests
47 #define LDEBUG3         0               // Debug NVMe to host queued requests (bus master)
48 #define LDEBUG4         0               // Xlinux PCIe DMA IP register debug
49
50 #include <NvmeAccess.h>
51
52 #define DMA_ID                          0x00
53 #define DMA_CONTROL                     0x04
54 #define DMA_STATUS                      0x40
55 #define DMA_COMPLETE                    0x48
56 #define DMA_ALIGNMENTS                  0x4C
57 #define DMA_WRITEBACK_ADDRESS_LOW       0x88
58 #define DMA_WRITEBACK_ADDRESS_HIGH      0x8C
59 #define DMA_INT_MASK                    0x90
60
61 #define DMASC_ID                        0x00
62 #define DMASC_ADDRESS_LOW               0x80
63 #define DMASC_ADDRESS_HIGH              0x84
64 #define DMASC_NEXT                      0x88
65 #define DMASC_CREDITS                   0x8C
66
67 /// Start nvmeProcess thread
68 static void* nvmeProcess(void* arg){
69         NvmeAccess*     nvmeAccess = (NvmeAccess*)arg;
70         
71         nvmeAccess->nvmeProcess();
72         return 0;
73 }
74
75 NvmeAccess::NvmeAccess(){
76         oregsFd = -1;
77         ohostSendFd = -1;
78         ohostRecvFd = -1;
79         oregs = 0;
80         obufTx1 = 0;
81         obufTx2 = 0;
82         obufRx = 0;
83         otag = 0;
84         onvmeNum = 0;
85         onvmeRegbase = 0x100;
86         oqueueNum = 16;
87         oqueueAdminRx = 0;
88         oqueueAdminTx = 0;
89         oqueueAdminId = 0;
90         oqueueDataRx = 0;
91         oqueueDataTx = 0;
92 }
93
94 NvmeAccess::~NvmeAccess(){
95         close();
96 }
97
98 void NvmeAccess::close(){
99         if(obufRx)
100                 free(obufRx);
101         if(obufTx2)
102                 free(obufTx2);
103         if(obufTx1)
104                 free(obufTx1);
105
106         if(odmaRegs)
107                 munmap((void*)odmaRegs, 4096);
108         if(oregs)
109                 munmap((void*)oregs, 4096);
110         
111         if(ohostRecvFd >= 0)
112                 ::close(ohostRecvFd);
113         if(ohostSendFd >= 0)
114                 ::close(ohostSendFd);
115         if(oregsFd >= 0)
116                 ::close(oregsFd);
117 }
118
119
120 int NvmeAccess::init(){
121         int     r;
122
123         if((oregsFd = open("/dev/bfpga0", O_RDWR | O_SYNC)) < 0){
124                 fprintf(stderr, "Unable to open /dev/xdma0_user\n");
125                 return 1;
126         }
127
128         if((r = ioctl(oregsFd, BFPGA_CMD_GETINFO, &oinfo)) < 0){
129                 fprintf(stderr, "Error ioctl: %s\n", strerror(errno));
130                 return 1;
131         }
132         dl1printf("Driver Register Addresses: %x(%x)\n", oinfo.regs.physAddress, oinfo.regs.length);
133
134         if((oregs = (volatile BUInt32*)mmap(0, oinfo.regs.length, PROT_READ|PROT_WRITE, MAP_SHARED, oregsFd, oinfo.regs.physAddress)) == 0){
135                 fprintf(stderr, "Error mmap: %s\n", strerror(errno));
136                 return 1;
137         }
138         
139         if((odmaRegs = (volatile BUInt32*)mmap(0, oinfo.dmaRegs.length, PROT_READ|PROT_WRITE, MAP_SHARED, oregsFd, oinfo.dmaRegs.physAddress)) == 0){
140                 fprintf(stderr, "Error mmap: %s\n", strerror(errno));
141                 return 1;
142         }
143
144
145         if((ohostSendFd = open("/dev/bfpga0-send0", O_RDWR)) < 0){
146                 fprintf(stderr, "Unable to open /dev/bfpga0-send0\n");
147                 return 1;
148         }
149
150         if((ohostRecvFd = open("/dev/bfpga0-recv0", O_RDWR)) < 0){
151                 fprintf(stderr, "Unable to open /dev/bfpga0-recv0\n");
152                 return 1;
153         }
154
155         posix_memalign((void **)&obufTx1, 4096, 4096);
156         posix_memalign((void **)&obufTx2, 4096, 4096);
157         posix_memalign((void **)&obufRx, 4096, 4096);
158
159         // Start of NVme request processing
160         pthread_create(&othread, 0, ::nvmeProcess, this);
161         
162         // Wait for this to have started
163         usleep(100000);
164
165         return 0;
166 }
167
168 void NvmeAccess::setNvme(BUInt n){
169         onvmeNum = n;
170         if(onvmeNum == 0)
171                 onvmeRegbase = 0x100;
172         else if(onvmeNum == 1)
173                 onvmeRegbase = 0x200;
174         else
175                 onvmeRegbase = 0x000;
176 }
177
178 #ifdef ZAP
179 void NvmeAccess::reset(){
180         BUInt32 data;
181
182         dl1printf("NvmeAccess::reset\n");
183         writeNvmeStorageReg(4, 0x00000001);
184
185         data = 1;
186         while(data & 3){
187                 data = readNvmeStorageReg(8);
188                 usleep(1000);
189         }
190         usleep(100000);
191
192         data = 0x06;
193         pcieWrite(10, 4, 1, &data);                     ///< Set PCIe config command for memory accesses
194 }
195 #else
196 void NvmeAccess::reset(){
197         BUInt32 data;
198         double  ts, te;
199
200         dl1printf("NvmeAccess::reset\n");
201         ts = getTime();
202
203         printf("Status: %8.8x\n", readNvmeStorageReg(RegStatus));
204         writeNvmeStorageReg(4, 0x00000001);
205         printf("Status: %8.8x\n", readNvmeStorageReg(RegStatus));
206
207         data = 1;
208         while(data & 1){
209                 data = readNvmeStorageReg(RegStatus);
210                 usleep(1000);
211         }
212         te = getTime();
213         printf("Reset time was: %f ms\n", (te - ts) * 1000);
214         usleep(100000);
215
216         printf("Last status was: %8.8x\n", data);
217         
218         if(UseConfigEngine){
219                 data = 0;
220                 while((data & 4) == 0){
221                         data = readNvmeStorageReg(RegStatus);
222                         usleep(1000);
223                 }
224                 te = getTime();
225                 printf("Full Reset time was: %f ms\n", (te - ts) * 1000);
226
227                 usleep(100000);
228                 printf("Last status was: %8.8x\n", data);
229         }
230         else {
231                 data = 0x06;
232                 pcieWrite(10, 4, 1, &data);                     ///< Set PCIe config command for memory accesses
233         }
234         usleep(100000);
235 }
236 #endif
237
238 // Send a queued request to the Nvme
239 int NvmeAccess::nvmeRequest(Bool wait, int queue, int opcode, BUInt32 address, BUInt32 arg10, BUInt32 arg11, BUInt32 arg12){
240         int     e;
241         BUInt32 cmd[16];
242         BUInt32 nvmeAddress;
243
244         memset(cmd, 0, 64);
245         cmd[0] = (0x01 << 24) | ((++oqueueAdminId & 0xFF) << 16) | opcode;      // This includes the hosts stream number
246         cmd[1] = queue ? 1:0;   // Namespace
247         cmd[2] = 0;             // Reserved
248         cmd[3] = 0;
249         cmd[4] = 0x00;          // Metadata
250         cmd[5] = 0x00;
251         cmd[6] = address;               // PRP1
252         cmd[7] = 0x00000000;
253         cmd[8] = address + 4096;        // PRP2
254         cmd[9] = 0x00000000;
255         cmd[10] = arg10;        // The argument CMD10
256         cmd[11] = arg11;        // The argument CMD11
257         cmd[12] = arg12;        // The argument CMD12
258
259 #ifdef ZAP
260         // Scatter gather lists are only supported on some Nvme's       
261         if(queue){
262                 // Use SGL
263                 cmd[0] |= (1 << 14);
264                 cmd[8] = 0x10000;               // Length of data
265         }
266 #endif
267
268         dl1printf("nvmeRequest:\n"); dl1hd32(cmd, 16);
269         oqueueReplySem.wait(0);
270
271         if(UseQueueEngine){
272                 // Send message to queue engine
273                 nvmeAddress = ((onvmeNum == 1) ? 0x10000000 : 0) | 0x02000000 | (queue << 16);
274                 dl2printf("Write to queue: %8.8x\n", nvmeAddress);
275                 if(e = pcieWrite(1, nvmeAddress, 16, cmd))
276                         return e;
277         }
278         else {
279                 if(queue){
280                         memcpy(&oqueueDataMem[oqueueDataTx * 16], cmd, sizeof(cmd));
281
282                         dl2printf("Submit IO: queue: %d 0x%x to slot: %d\n", queue, opcode, oqueueDataTx);
283                         dl2hd32(cmd, 64 / 4);
284
285                         oqueueDataTx++;
286                         if(oqueueDataTx >= oqueueNum)
287                                 oqueueDataTx = 0;
288
289                         if(e = writeNvmeReg32(0x1008, oqueueDataTx)){
290                                 printf("Error: %d\n", e);
291                                 return 1;
292                         }
293                 }
294                 else {
295                         memcpy(&oqueueAdminMem[oqueueAdminTx * 16], cmd, sizeof(cmd));
296
297                         dl2printf("Submit command: queue: %d opcode: 0x%x to slot: %d\n", queue, opcode, oqueueAdminTx);
298                         dl2hd32(cmd, 64 / 4);
299                 
300                         oqueueAdminTx++;
301                         if(oqueueAdminTx >= oqueueNum)
302                                 oqueueAdminTx = 0;
303
304                         if(e = writeNvmeReg32(0x1000, oqueueAdminTx)){
305                                 printf("Error: %d\n", e);
306                                 return 1;
307                         }
308                 }
309         }
310         
311         if(wait){
312                 // Wait for reply
313                 oqueueReplySem.wait();
314         }
315
316         return 0;
317 }
318
319 /// This function runs as a separate thread in order to receive both replies and requests from the Nvme.
320 int NvmeAccess::nvmeProcess(){
321         int                     nt;
322         NvmeRequestPacket       request;
323         NvmeReplyPacket         reply;
324         BUInt32*                data;
325         BUInt32                 nWordsRet;
326         BUInt32                 nWords;
327         int                     e;
328         int                     status = 0;
329         
330         // This reads packets from the NVMe and processes them. The packets have a special requester header produced by the Xilinx PCIe DMA IP.
331         // Responces have the special completer header added for the Xilinx PCIe DMA IP.
332         while(1){
333                 dl3printf("NvmeAccess::nvmeProcess: loop\n");
334
335                 // Read the packet from the Nvme. Coupdl be a request or a reply
336                 nt = read(ohostRecvFd, obufRx, 4096);
337
338                 dl3printf("NvmeAccess::nvmeProcess: awoken with: %d bytes\n", nt);
339                 //dl3hd32(obufRx, nt / 4);
340                 //printf("NvmeAccess::nvmeProcess: awoken with: %d bytes\n", nt);
341                 //bhd32(obufRx, nt / 4);
342
343                 // Determine if packet is a reply or an Nvme request from the reply bit in the header
344                 if(obufRx[2] & 0x80000000){
345                         memcpy(&opacketReply, obufRx, sizeof(opacketReply));
346                         dl3printf("NvmeAccess::nvmeProcess: Reply id: %x\n", opacketReply.requesterId);
347                         dl3hd32(&opacketReply, nt / 4);
348                         opacketReplySem.set();
349                         continue;
350                 }
351                 else {
352                         memcpy(&request, obufRx, sizeof(request));
353                 }
354                 
355                 dl3printf("NvmeAccess::nvmeProcess: recvNum: %d Req: %d nWords: %d address: 0x%8.8x\n", nt, request.request, request.numWords, request.address);
356                 dl3hd32(&request, nt / 4);
357                 //dumpStatus();
358
359                 if(request.request == 0){
360                         // PCIe Read requests
361                         dl3printf("NvmeAccess::nvmeProcess: Read memory: address: %8.8x nWords: %d\n", request.address, request.numWords);
362                         if((request.address & 0x00FF0000) == 0x00000000){
363                                 data = oqueueAdminMem;
364                         }
365                         else if((request.address & 0x00FF0000) == 0x00010000){
366                                 data = oqueueDataMem;
367                         }
368                         else if((request.address & 0x00FF0000) == 0x00800000){
369                                 data = odataBlockMem;
370                         }
371                         else {
372                                 printf("NvmeAccess::nvmeProcess: Error read from uknown address: 0x%8.8x\n", request.address);
373                                 continue;
374                         }
375
376                         nWordsRet = request.numWords;
377                         while(nWordsRet){
378                                 nWords = nWordsRet;
379                                 if(nWords > PcieMaxPayloadSize)
380                                         nWords = PcieMaxPayloadSize;
381
382                                 memset(&reply, 0, sizeof(reply));
383                                 if(onvmeNum == 1)
384                                         reply.completerId = 0x0100;
385                                 reply.reply = 1;
386                                 reply.address = request.address & 0x0FFF;
387                                 reply.numBytes = (nWordsRet * 4);
388                                 reply.numWords = nWords;
389                                 reply.tag = request.tag;
390                                 memcpy(reply.data, &data[(request.address & 0x0000FFFF) / 4], nWords * 4);
391
392                                 dl3printf("NvmeAccess::nvmeProcess: ReadData block from: 0x%8.8x nWords: %d\n", request.address, nWords);
393                                 dl3hd32(&reply, (3 + nWords));
394                                 if(packetSend(reply)){
395                                         printf("NvmeAccess::nvmeProcess: packet send error\n");
396                                         exit(1);
397                                 }
398                                         
399                                 nWordsRet -= nWords;
400                                 request.address += (4 * nWords);
401                         }
402                 }
403                 else if(request.request == 1){
404                         // PCIe Write requests
405                         dl3printf("NvmeAccess::nvmeProcess: Write memory: address: %8.8x nWords: %d\n", request.address, request.numWords);
406                         status = 0;
407                         
408                         if((request.address & 0x00FF0000) == 0x00100000){
409                                 status = request.data[3] >> 17;
410                                 dl3printf("NvmeAccess::nvmeProcess: NvmeReply: Queue: %d QueueHeadPointer: %d Status: 0x%4.4x Command: 0x%x\n", request.data[2] >> 16, request.data[2] & 0xFFFF, request.data[3] >> 17, request.data[3] & 0xFFFF);
411                                 //printf("NvmeAccess::nvmeProcess: NvmeReply: Queue: %d QueueHeadPointer: %d Status: 0x%4.4x Command: 0x%x\n", request.data[2] >> 16, request.data[2] & 0xFFFF, request.data[3] >> 17, request.data[3] & 0xFFFF);
412                                 //bhd32(&request, nt / 4);
413
414                                 // Write to completion queue doorbell
415                                 oqueueAdminRx++;
416                                 if(oqueueAdminRx >= oqueueNum)
417                                         oqueueAdminRx = 0;
418
419                                 if(!UseQueueEngine){
420                                         dl3printf("NvmeAccess::nvmeProcess: Write completion queue doorbell: %d\n", oqueueAdminRx);
421                                         printf("NvmeAccess::nvmeProcess: Write completion queue doorbell: %d\n", oqueueAdminRx);
422                                         if(e = writeNvmeReg32(0x1004, oqueueAdminRx)){
423                                                 printf("Error: %d\n", e);
424                                                 return 1;
425                                         }
426                                 }
427                                 oqueueReplySem.set();
428                         }
429                         else if((request.address & 0x00FF0000) == 0x00110000){
430                                 status = request.data[3] >> 17;
431                                 dl3printf("NvmeAccess::nvmeProcess: IoCompletion: Queue: %d QueueHeadPointer: %d Status: 0x%4.4x Command: 0x%x\n", request.data[2] >> 16, request.data[2] & 0xFFFF, request.data[3] >> 17, request.data[3] & 0xFFFF);
432                                 //printf("NvmeAccess::nvmeProcess: IoCompletion: Queue: %d QueueHeadPointer: %d Status: 0x%4.4x Command: 0x%x\n", request.data[2] >> 16, request.data[2] & 0xFFFF, request.data[3] >> 17, request.data[3] & 0xFFFF);
433
434                                 // Write to completion queue doorbell
435                                 oqueueDataRx++;
436                                 if(oqueueDataRx >= oqueueNum)
437                                         oqueueDataRx = 0;
438
439                                 if(!UseQueueEngine){
440                                         dl3printf("NvmeAccess::nvmeProcess: Write completion queue doorbell: %d\n", oqueueDataRx);
441                                         if(e = writeNvmeReg32(0x100C, oqueueDataRx)){
442                                                 printf("Error: %d\n", e);
443                                                 return 1;
444                                         }
445                                 }
446                                 oqueueReplySem.set();
447                         }
448                         else if((request.address & 0x00FF0000) == 0x000800000){
449                                 dl3printf("NvmeAccess::nvmeProcess: IoBlockWrite: address: %8.8x nWords: %d\n", (request.address & 0x0FFFFFFF), request.numWords);
450                                 //printf("NvmeAccess::nvmeProcess: IoBlockWrite: address: %8.8x nWords: %d\n", (request.address & 0x0FFFFFFF), request.numWords);
451
452                                 memcpy(&odataBlockMem[(request.address & 0x0000FFFF) / 4], request.data, request.numWords * 4);
453                         }
454                         else if((request.address & 0x00FF0000) == 0x00F00000){
455                                 printf("NvmeAccess::nvmeProcess: Write: address: %8.8x nWords: %d\n", (request.address & 0x0FFFFFFF), nWords);
456                                 memcpy(&odataBlockMem[(request.address & 0x0000FFFF) / 4], request.data, request.numWords * 4);
457                                 bhd32(odataBlockMem, request.numWords);
458                         }
459                         else {
460                                 printf("NvmeAccess::nvmeProcess: Write data: unknown address: 0x%8.8x\n", request.address);
461                         }
462                         
463                         if(status){
464                                 printf("NvmeAccess::nvmeProcess: Queue Command returned error: status: %4.4x\n", status);
465                                 bhd32(&request, nt / 4);
466                         }
467                 }
468                 else {
469                         printf("NvmeAccess::nvmeProcess: Error: Uknown request: %x\n", request.request);
470                 }
471         }
472
473         return 0;
474 }
475
476 BUInt32 NvmeAccess::readNvmeStorageReg(BUInt32 address){
477         return oregs[onvmeRegbase/4 + address/4];
478 }
479
480 void NvmeAccess::writeNvmeStorageReg(BUInt32 address, BUInt32 data){
481         oregs[onvmeRegbase/4 + address/4] = data;
482 }
483
484 int NvmeAccess::readNvmeReg32(BUInt32 address, BUInt32& data){
485         return pcieRead(0, address, 1, (BUInt32*)&data);
486 }
487
488 int NvmeAccess::writeNvmeReg32(BUInt32 address, BUInt32 data){
489         return pcieWrite(1, address, 1, (BUInt32*)&data);
490 }
491
492 int NvmeAccess::readNvmeReg64(BUInt32 address, BUInt64& data){
493         return pcieRead(0, address, 2, (BUInt32*)&data);
494 }
495
496 int NvmeAccess::writeNvmeReg64(BUInt32 address, BUInt64 data){
497         return pcieWrite(1, address, 2, (BUInt32*)&data);
498 }
499
500 int NvmeAccess::pcieWrite(BUInt8 request, BUInt32 address, BUInt32 num, BUInt32* data){
501         NvmeRequestPacket       txPacket;
502         int                     nt;
503         int                     reqType;
504         BUInt8                  err;
505
506         //printf("pcieWrite\n");
507         if(onvmeNum == 1){
508                 address |= 0x10000000;
509         }
510         
511         // Memory or Config read
512         dl2printf("NvmeAccess::pcieWrite address: 0x%8.8x num: %d\n", address, num);
513         txPacket.request = request;             // The request to perform
514         txPacket.address = address;             // 32bit address
515         txPacket.numWords = num;                // Number of 32bit DWords
516         txPacket.tag = ++otag;                  // Tag
517         txPacket.requesterId = 0x0001;          // The hosts stream
518         txPacket.requesterIdEnable = 1;         // Enable requestor ID's
519         
520         memcpy(txPacket.data, data, (num * 4));
521
522         dl2printf("Send packet\n");
523         dl2hd32(&txPacket, 4 + num);
524
525 #if LDEBUG4
526         dumpDmaRegs(0, 0);
527         dumpDmaRegs(1, 0);
528 #endif
529         if(packetSend(txPacket)){
530                 printf("Packet send error\n");
531                 return 1;
532         }       
533
534         if(request == 10){
535                 // Wait for a reply on config write requests
536                 opacketReplySem.wait();
537                 dl2printf("Received reply: status: %x, error: %x, numWords: %d\n", opacketReply.status, opacketReply.error, opacketReply.numWords);
538                 opacketReply.numWords++;
539                 
540                 dl2hd32(&opacketReply, 3 + opacketReply.numWords);
541                 if(opacketReply.error)
542                         return opacketReply.error;
543         }
544         else {
545                 // Not sure why this is needed ?
546                 //usleep(1000);
547         }
548         
549         return 0;
550 }
551
552 int NvmeAccess::pcieRead(BUInt8 request, BUInt32 address, BUInt32 num, BUInt32* data){
553         NvmeRequestPacket       txPacket;
554         BUInt8                  err;
555         int                     nt = num;
556
557         if(onvmeNum == 1){
558                 address |= 0x10000000;
559         }
560
561         // Memory or Config read
562         dl1printf("NvmeAccess::pcieRead read: address: %d num: %d\n", address, num);
563         txPacket.request = request;             // The request to perform
564         txPacket.address = address;             // 32bit address
565         txPacket.numWords = num;                // Number of 32bit DWords
566         txPacket.tag = ++otag;                  // Tag
567         txPacket.requesterId = 0x0001;          // The hosts stream
568         txPacket.requesterIdEnable = 1;         // Enable requestor ID's
569         
570         dl2printf("NvmeAccess::pcieRead: Send packet\n");
571         dl2hd32(&txPacket, 4);
572
573 #if LDEBUG4
574         dumpDmaRegs(0, 0);
575         dumpDmaRegs(1, 0);
576 #endif
577         memset(obufRx, 0, 4096);
578
579         if(packetSend(txPacket)){
580                 printf("Packet send error\n");
581                 return 1;
582         }       
583
584         dl2printf("Recv data\n");
585         
586 #if LDEBUG4
587         usleep(100000);
588         dumpDmaRegs(0, 0);
589         dumpDmaRegs(1, 0);
590 #endif
591         
592 #ifdef ZAP
593         nt = read(ohostRecvFd, obufRx, 4096);
594         dl2printf("Read %d\n", nt);
595
596         if(nt > 0)
597                 dl2hd32(obufRx, nt / 4);
598
599         pause();
600 #endif
601
602         // Wait for a reply
603         opacketReplySem.wait();
604         dl2printf("Received reply: status: %x, error: %x, numWords: %d\n", opacketReply.status, opacketReply.error, opacketReply.numWords);
605         
606         dl2hd32(&opacketReply, 3 + opacketReply.numWords);
607         if(opacketReply.error)
608                 return opacketReply.error;
609
610         memcpy(data, opacketReply.data, (num * sizeof(BUInt32)));
611
612         return 0;
613 }
614
615 int NvmeAccess::packetSend(const NvmeRequestPacket& packet){
616         BUInt   nb = 16;
617
618         if((packet.request == 1) || (packet.request == 10) || (packet.request == 12))
619                 nb += (4 * packet.numWords);
620
621         memcpy(obufTx1, &packet, nb);
622         //printf("SendPacket: numWords: %d %d\n", packet.numWords, nb);
623         //bhd32(obufTx1, nb/4);
624
625         if(write(ohostSendFd, obufTx1, nb) != nb){
626                 printf("Send error\n");
627                 return 1;
628         }
629         return 0;
630 }
631
632 int NvmeAccess::packetSend(const NvmeReplyPacket& packet){
633         BUInt   nb = 12 + (4 * packet.numWords);
634
635         memcpy(obufTx2, &packet, nb);
636         //printf("NvmeAccess::packetSend: reply: nWords: %d nBytes: %d\n", packet.numWords, nb);
637         //bhd32(obufTx2, nb / 4);
638
639         if(write(ohostSendFd, obufTx2, nb) != nb){
640                 printf("Send error\n");
641                 return 1;
642         }
643         return 0;
644 }
645
646
647 void NvmeAccess::dumpRegs(int nvmeNum){
648         int     r;
649         BUInt32 nvmeRegbase;
650         
651         if(nvmeNum == 0)
652                 nvmeRegbase = 0x100;
653         else if(nvmeNum == 1)
654                 nvmeRegbase = 0x200;
655         else if(nvmeNum == 2)
656                 nvmeRegbase = 0x000;
657         else
658                 nvmeRegbase = onvmeRegbase;
659
660         printf("NvmeStorageUnit's registers: base: 0x%x\n", nvmeRegbase);
661         printf("Id:             %8.8x\n", oregs[nvmeRegbase/4 + 0]);
662         printf("Control:        %8.8x\n", oregs[nvmeRegbase/4 + 1]);
663         printf("Status:         %8.8x\n", oregs[nvmeRegbase/4 + 2]);
664         printf("TotalBlocks:    %8.8x\n", oregs[nvmeRegbase/4 + 3]);
665         printf("DataChunkStart: %8.8x\n", oregs[nvmeRegbase/4 + 16]);
666         printf("DataChunkSize:  %8.8x\n", oregs[nvmeRegbase/4 + 17]);
667         printf("Error:          %8.8x\n", oregs[nvmeRegbase/4 + 18]);
668         printf("NumBlocks:      %8.8x\n", oregs[nvmeRegbase/4 + 19]);
669         printf("TimeUs:         %8.8x\n", oregs[nvmeRegbase/4 + 20]);
670         printf("PeakLatencyUs:  %8.8x\n", oregs[nvmeRegbase/4 + 21]);
671
672         printf("ReadControl:    %8.8x\n", oregs[nvmeRegbase/4 + 32]);
673         printf("ReadStatus:     %8.8x\n", oregs[nvmeRegbase/4 + 33]);
674         printf("ReadBlock:      %8.8x\n", oregs[nvmeRegbase/4 + 34]);
675         printf("ReadNumBlocks:  %8.8x\n", oregs[nvmeRegbase/4 + 35]);
676
677 #ifdef ZAP      
678         for(r = 16; r < 21; r++){
679                 printf("Reg%2.2d:    %8.8x\n", r, oregs[nvmeRegbase/4 + r]);
680         }
681 #endif
682 }
683
684 void  NvmeAccess::dumpDmaRegs(bool c2h, int chan){
685         int                     regsAddress = (c2h << 12) | (chan << 8);
686         int                     sgregsAddress = ((4 + c2h) << 12) | (chan << 8);
687         volatile BUInt32*       regs = &odmaRegs[regsAddress / 4];
688         volatile BUInt32*       sgregs = &odmaRegs[sgregsAddress / 4];
689         
690         printf("DMA Channel:    %d.%d\n", c2h, chan);
691         //printf("DMA regs:       0x%x\n", regsAddress);
692         printf("DMA_ID:         %x\n", regs[DMA_ID / 4]);
693         printf("DMA_CONTROL:    %x\n", regs[DMA_CONTROL / 4]);
694         printf("DMA_STATUS:     %x\n", regs[DMA_STATUS / 4]);
695         printf("DMA_COMPLETE:   %x\n", regs[DMA_COMPLETE / 4]);
696         printf("DMA_INT_MASK:   %x\n", regs[DMA_INT_MASK / 4]);
697
698         if(0){  
699                 printf("DMASC_ID:               %x\n", sgregs[DMASC_ID / 4]);
700                 //printf("DMASC regs:             0x%x\n", sgregsAddress);
701                 printf("DMASC_ADDRESS_LOW:      %x\n", sgregs[DMASC_ADDRESS_LOW / 4]);
702                 printf("DMASC_ADDRESS_HIGH:     %x\n", sgregs[DMASC_ADDRESS_HIGH / 4]);
703                 printf("DMASC_NEXT:             %x\n", sgregs[DMASC_NEXT / 4]);
704 #ifdef ZAP
705                 printf("SGmemory\n");
706                 if(chan)
707                         bhd32((void*)dma1Mem, 64);
708                 else
709                         bhd32((void*)dma0Mem, 64);
710 #endif
711         }
712 }
713
714 void NvmeAccess::dumpStatus(){
715         BUInt32 data;
716         int     e;
717         
718         if(e = readNvmeReg32(0x1C, data)){
719                 printf("Error: %d\n", e);
720                 return;
721         }
722         printf("StatusReg: 0x%3.3x 0x%8.8x\n", 0x1C, data);
723 }