Use host virtual addr for SQ commands

hw/block/femu/femu.c

@@ -6,6 +6,7 @@
 #include "qapi/visitor.h"
 #include "qapi/error.h"
 
+#include <immintrin.h>
 #include "nvme.h"
 
 /* Coperd: IO thread */
@@ -271,16 +272,39 @@ static uint16_t nvme_io_cmd(FemuCtrl *n, NvmeCmd *cmd, NvmeRequest *req)
     }
 }
 
-/* Coperd: eventidx buffer is not needed */
-#if 0
 static void nvme_update_sq_eventidx(const NvmeSQueue *sq)
 {
+    if (sq->eventidx_addr_hva) {
+        *((uint32_t *)(sq->eventidx_addr_hva)) = sq->tail;
+        return;
+    }
+
     if (sq->eventidx_addr) {
         nvme_addr_write(sq->ctrl, sq->eventidx_addr, (void *)&sq->tail,
-            sizeof(sq->tail));
+                sizeof(sq->tail));
     }
 }
+
+static inline void nvme_copy_cmd(NvmeCmd *dst, NvmeCmd *src)
+{
+#if defined(__AVX__)
+    __m256i *d256 = (__m256i *)dst;
+    const __m256i *s256 = (const __m256i *)src;
+
+    _mm256_store_si256(&d256[0], _mm256_load_si256(&s256[0]));
+    _mm256_store_si256(&d256[1], _mm256_load_si256(&s256[1]));
+#elif defined(__SSE2__)
+    __m128i *d128 = (__m128i *)dst;
+    const __m128i *s128 = (const __m128i *)src;
+
+    _mm_store_si128(&d128[0], _mm_load_si128(&s128[0]));
+    _mm_store_si128(&d128[1], _mm_load_si128(&s128[1]));
+    _mm_store_si128(&d128[2], _mm_load_si128(&s128[2]));
+    _mm_store_si128(&d128[3], _mm_load_si128(&s128[3]));
+#else
+    *dst = *src;
 #endif
+}
 
 void nvme_process_sq_io(void *opaque)
 {
@@ -298,24 +322,24 @@ void nvme_process_sq_io(void *opaque)
     while (!(nvme_sq_empty(sq))) {
         if (sq->phys_contig) {
             addr = sq->dma_addr + sq->head * n->sqe_size;
+            nvme_copy_cmd(&cmd, (void *)&(((NvmeCmd *)sq->dma_addr_hva)[sq->head]));
         } else {
             addr = nvme_discontig(sq->prp_list, sq->head, n->page_size,
                     n->sqe_size);
+            nvme_addr_read(n, addr, (void *)&cmd, sizeof(cmd));
         }
-        nvme_addr_read(n, addr, (void *)&cmd, sizeof(cmd));
         nvme_inc_sq_head(sq);
 
         if (cmd.opcode == NVME_OP_ABORTED) {
-            printf("Coperd,abort!!!!\n");
+            printf("Coperd,abort!!!! Please report this as a bug !\n");
             continue;
         }
         req = QTAILQ_FIRST(&sq->req_list);
         memset(&req->cqe, 0, sizeof(req->cqe));
         req->cqe.cid = cmd.cid;
-        req->aiocb = NULL;
 
         status = nvme_io_cmd(n, &cmd, req);
-        if (status != NVME_NO_COMPLETE) {
+        if (status == NVME_SUCCESS) {
             req->status = status;
             nvme_post_cqe(cq, req);
         } else {
@@ -333,18 +357,8 @@ void nvme_process_sq_io(void *opaque)
         n->completed = 0;
     }
 
-    /*
-     * Coperd: no need to keep the tail up-to-date with guest, we will handle
-     * newly submitted I/Os during next sq->timer triggering
-     */
-#if 0
     nvme_update_sq_eventidx(sq);
-    nvme_update_sq_tail(sq);
-#endif
-
     sq->completed += processed;
-
-    //timer_mod(sq->timer, qemu_clock_get_ns(QEMU_CLOCK_REALTIME) + SQ_POLLING_PERIOD_NS);
 }
 
 static void nvme_clear_ctrl(FemuCtrl *n, bool shutdown)

hw/block/femu/nvme-core.c

@@ -56,8 +56,10 @@ uint16_t nvme_init_sq(NvmeSQueue *sq, FemuCtrl *n, uint64_t dma_addr,
 {
     uint8_t stride = n->db_stride;
     int dbbuf_entry_sz = 1 << (2 + stride);
-    int i;
+    AddressSpace *as = pci_get_address_space(&n->parent_obj);
+    dma_addr_t sqsz = (dma_addr_t)size;
     NvmeCQueue *cq;
+    int i;
 
     sq->ctrl = n;
     sq->sqid = sqid;
@@ -67,6 +69,7 @@ uint16_t nvme_init_sq(NvmeSQueue *sq, FemuCtrl *n, uint64_t dma_addr,
     sq->phys_contig = contig;
     if (sq->phys_contig) {
         sq->dma_addr = dma_addr;
+        sq->dma_addr_hva = (uint64_t)dma_memory_map(as, dma_addr, &sqsz, 0);
     } else {
         sq->prp_list = nvme_setup_discontig(n, dma_addr, size, n->sqe_size);
         if (!sq->prp_list) {
@@ -209,9 +212,12 @@ uint16_t nvme_init_cq(NvmeCQueue *cq, FemuCtrl *n, uint64_t dma_addr,
 
     uint8_t stride = n->db_stride;
     int dbbuf_entry_sz = 1 << (2 + stride);
+    AddressSpace *as = pci_get_address_space(&n->parent_obj);
+    dma_addr_t cqsz = (dma_addr_t)size;
 
     if (cq->phys_contig) {
         cq->dma_addr = dma_addr;
+        cq->dma_addr_hva = (uint64_t)dma_memory_map(as, dma_addr, &cqsz, 1);
     } else {
         cq->prp_list = nvme_setup_discontig(n, dma_addr, size,
                 n->cqe_size);

hw/block/femu/nvme-util.c

@@ -183,7 +183,8 @@ void nvme_inc_sq_head(NvmeSQueue *sq)
 void nvme_update_cq_head(NvmeCQueue *cq)
 {
     if (cq->db_addr_hva) {
-        memcpy(&cq->head, (void *)cq->db_addr_hva, sizeof(cq->head));
+        cq->head = *(uint32_t *)(cq->db_addr_hva);
+        //memcpy(&cq->head, (void *)cq->db_addr_hva, sizeof(cq->head));
         return;
     }
 
@@ -688,7 +689,8 @@ uint64_t nvme_cmb_read(void *opaque, hwaddr addr, unsigned size)
 void nvme_update_sq_tail(NvmeSQueue *sq)
 {
     if (sq->db_addr_hva) {
-        memcpy(&sq->tail, (void *)sq->db_addr_hva, sizeof(sq->tail));
+        sq->tail = *((uint32_t *)sq->db_addr_hva);
+        //memcpy(&sq->tail, (void *)sq->db_addr_hva, sizeof(sq->tail));
         return;
     }
 

hw/block/femu/nvme.h

@@ -740,7 +740,6 @@ typedef struct NvmeAsyncEvent {
 typedef struct NvmeRequest {
     struct NvmeSQueue       *sq;
     struct NvmeNamespace    *ns;
-    BlockAIOCB               *aiocb;
     uint16_t                status;
     uint64_t                slba;
     uint16_t                is_write;
@@ -752,15 +751,13 @@ typedef struct NvmeRequest {
     uint64_t                femu_oc_slba;
     uint64_t                *femu_oc_ppa_list;
     NvmeCqe                 cqe;
-    BlockAcctCookie         acct;
     QEMUSGList              qsg;
     QEMUIOVector            iov;
     QTAILQ_ENTRY(NvmeRequest)entry;
     int64_t                 expire_time;
     int64_t                 data_offset;
     int                     lunid;
     int                     chnl;
-
 } NvmeRequest;
 
 typedef struct DMAOff {
@@ -780,6 +777,7 @@ typedef struct NvmeSQueue {
     uint32_t    tail;
     uint32_t    size;
     uint64_t    dma_addr;
+    uint64_t    dma_addr_hva;
     uint64_t    completed;
     uint64_t    *prp_list;
     QEMUTimer   *timer;
@@ -807,6 +805,7 @@ typedef struct NvmeCQueue {
     uint32_t    vector;
     uint32_t    size;
     uint64_t    dma_addr;
+    uint64_t    dma_addr_hva;
     uint64_t    *prp_list;
     EventNotifier guest_notifier;
     QEMUTimer   *timer;