MdeModulePkg: NvmExpressDxe: Request queues from

VivianNK · VivianNK · commit b273e57b83b4 · 2025-01-28T14:50:58.000-08:00
the controller.

Instead of assuming we always want to create two sets of data queues,
we request two and the controller will determine how many are allocated.

Signed-off-by: Vivian Nowka-Keane &lt;vnowkakeane@linux.microsoft.com&gt;
diff --git a/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpress.c b/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpress.c
@@ -74,11 +74,13 @@ EnumerateNvmeDevNamespace (
   UINT32                     LbaFmtIdx;
   UINT8                      Sn[21];
   UINT8                      Mn[41];
+  BOOLEAN                    MultipleDataQueues;
   VOID                       *DummyInterface;
 
-  NewDevicePathNode = NULL;
-  DevicePath        = NULL;
-  Device            = NULL;
+  NewDevicePathNode  = NULL;
+  DevicePath         = NULL;
+  Device             = NULL;
+  MultipleDataQueues = FALSE;
 
   //
   // Allocate a buffer for Identify Namespace data
@@ -107,6 +109,20 @@ EnumerateNvmeDevNamespace (
   if (NamespaceData->Ncap == 0) {
     Status = EFI_DEVICE_ERROR;
   } else {
+    //
+    // Send Set Features Command to request the maximum number of data queues
+    //
+    Status = NvmeSetFeaturesNumberOfQueues (Private, NVME_MAX_QUEUES - 1, NVME_MAX_QUEUES - 1);
+
+    if (EFI_ERROR (Status)) {
+      goto Exit;
+    }
+
+    if (Private->NSQA > 1) {
+      // There are multiple data queue pairs (non-admin queues) available
+      MultipleDataQueues = TRUE;
+    }
+
     //
     // allocate device private data for each discovered namespace
     //
@@ -172,15 +188,17 @@ EnumerateNvmeDevNamespace (
     Device->BlockIo.WriteBlocks = NvmeBlockIoWriteBlocks;
     Device->BlockIo.FlushBlocks = NvmeBlockIoFlushBlocks;
 
-    //
-    // Create BlockIo2 Protocol instance
-    //
-    Device->BlockIo2.Media         = &Device->Media;
-    Device->BlockIo2.Reset         = NvmeBlockIoResetEx;
-    Device->BlockIo2.ReadBlocksEx  = NvmeBlockIoReadBlocksEx;
-    Device->BlockIo2.WriteBlocksEx = NvmeBlockIoWriteBlocksEx;
-    Device->BlockIo2.FlushBlocksEx = NvmeBlockIoFlushBlocksEx;
-    InitializeListHead (&Device->AsyncQueue);
+    if (MultipleDataQueues) {
+      //
+      // Create BlockIo2 Protocol instance
+      //
+      Device->BlockIo2.Media         = &Device->Media;
+      Device->BlockIo2.Reset         = NvmeBlockIoResetEx;
+      Device->BlockIo2.ReadBlocksEx  = NvmeBlockIoReadBlocksEx;
+      Device->BlockIo2.WriteBlocksEx = NvmeBlockIoWriteBlocksEx;
+      Device->BlockIo2.FlushBlocksEx = NvmeBlockIoFlushBlocksEx;
+      InitializeListHead (&Device->AsyncQueue);
+    }
 
     // MU_CHANGE Start - Add Media Sanitize
     //
@@ -260,8 +278,6 @@ EnumerateNvmeDevNamespace (
                     Device->DevicePath,
                     &gEfiBlockIoProtocolGuid,
                     &Device->BlockIo,
-                    &gEfiBlockIo2ProtocolGuid,
-                    &Device->BlockIo2,
                     &gEfiDiskInfoProtocolGuid,
                     &Device->DiskInfo,
                     NULL
@@ -271,6 +287,18 @@ EnumerateNvmeDevNamespace (
       goto Exit;
     }
 
+    if (MultipleDataQueues) {
+      Status = gBS->InstallProtocolInterface (
+                      &Device->DeviceHandle,
+                      &gEfiBlockIo2ProtocolGuid,
+                      EFI_NATIVE_INTERFACE, // this is the only option in the enum..?
+                      &Device->BlockIo2
+                      );
+      if (EFI_ERROR (Status)) {
+        goto Exit;
+      }
+    }
+
     //
     // Check if the NVMe controller supports the Security Send and Security Receive commands
     //
@@ -288,12 +316,19 @@ EnumerateNvmeDevNamespace (
                Device->DevicePath,
                &gEfiBlockIoProtocolGuid,
                &Device->BlockIo,
-               &gEfiBlockIo2ProtocolGuid,
-               &Device->BlockIo2,
                &gEfiDiskInfoProtocolGuid,
                &Device->DiskInfo,
                NULL
                );
+
+        if (MultipleDataQueues) {
+          gBS->UninstallProtocolInterface (
+                 Device->DeviceHandle,
+                 &gEfiBlockIo2ProtocolGuid,
+                 &Device->BlockIo2
+                 );
+        }
+
         goto Exit;
       }
     }
@@ -430,6 +465,7 @@ UnregisterNvmeNamespace (
 {
   EFI_STATUS                             Status;
   EFI_BLOCK_IO_PROTOCOL                  *BlockIo;
+  EFI_BLOCK_IO2_PROTOCOL                 *BlockIo2;
   NVME_DEVICE_PRIVATE_DATA               *Device;
   EFI_STORAGE_SECURITY_COMMAND_PROTOCOL  *StorageSecurity;
   BOOLEAN                                IsEmpty;
@@ -487,8 +523,6 @@ UnregisterNvmeNamespace (
                   Device->DevicePath,
                   &gEfiBlockIoProtocolGuid,
                   &Device->BlockIo,
-                  &gEfiBlockIo2ProtocolGuid,
-                  &Device->BlockIo2,
                   &gEfiDiskInfoProtocolGuid,
                   &Device->DiskInfo,
                   NULL
@@ -506,6 +540,37 @@ UnregisterNvmeNamespace (
     return Status;
   }
 
+  //
+  // If BlockIo2 is installed, uninstall it.
+  //
+  Status = gBS->OpenProtocol (
+                  Handle,
+                  &gEfiBlockIo2ProtocolGuid,
+                  (VOID **)&BlockIo2,
+                  This->DriverBindingHandle,
+                  Controller,
+                  EFI_OPEN_PROTOCOL_GET_PROTOCOL
+                  );
+
+  if (!EFI_ERROR (Status)) {
+    Status = gBS->UninstallProtocolInterface (
+                    Handle,
+                    &gEfiBlockIo2ProtocolGuid,
+                    &Device->BlockIo2
+                    );
+    if (EFI_ERROR (Status)) {
+      gBS->OpenProtocol (
+             Controller,
+             &gEfiNvmExpressPassThruProtocolGuid,
+             (VOID **)&DummyInterface,
+             This->DriverBindingHandle,
+             Handle,
+             EFI_OPEN_PROTOCOL_BY_CHILD_CONTROLLER
+             );
+      return Status;
+    }
+  }
+
   //
   // If Storage Security Command Protocol is installed, then uninstall this protocol.
   //
diff --git a/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpress.h b/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpress.h
@@ -78,7 +78,8 @@ extern EFI_DRIVER_SUPPORTED_EFI_VERSION_PROTOCOL  gNvmExpressDriverSupportedEfiV
 //
 #define NVME_ASYNC_CCQ_SIZE  255
 
-#define NVME_MAX_QUEUES  3                              // Number of queues supported by the driver
+// Maximum number of queue pairs supported by the driver, including the admin queues.
+#define NVME_MAX_QUEUES  3
 
 // MU_CHANGE Start - Add Media Sanitize
 //
@@ -161,6 +162,12 @@ struct _NVME_CONTROLLER_PRIVATE_DATA {
   //
   NVME_ADMIN_CONTROLLER_DATA            *ControllerData;
 
+  //
+  // Number of Queues Allocated by the controller (0-based where 0 is 1 queue)
+  // 
+  UINT32 NSQA; // Number of Submission Queues Allocated
+  UINT32 NCQA; // Number of Completion Queues Allocated
+
   //
   // 6 x 4kB aligned buffers will be carved out of this buffer.
   // 1st 4kB boundary is the start of the admin submission queue.
diff --git a/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressHci.c b/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressHci.c
@@ -567,7 +567,88 @@ NvmeIdentifyNamespace (
 }
 
 /**
-  Create io completion queue.
+  Send the Set Features Command to the controller for the number of queues requested.
+  Note that the number of queues allocated may be less than the number of queues requested.
+
+  @param  Private          The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
+  @param  NCQR             The number of completion queues requested
+  @param  NSQR             The number of submission queues requested
+
+  @return EFI_SUCCESS      Successfully set the number of queues.
+  @return EFI_DEVICE_ERROR Fail to set the number of queues.
+
+**/
+EFI_STATUS
+NvmeSetFeaturesNumberOfQueues (
+  IN NVME_CONTROLLER_PRIVATE_DATA  *Private,
+  IN UINT16                        NSQR,
+  IN UINT16                        NCQR
+  )
+{
+  EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET  CommandPacket;
+  EFI_NVM_EXPRESS_COMMAND                   Command;
+  EFI_NVM_EXPRESS_COMPLETION                Completion;
+  EFI_STATUS                                Status;
+  NVME_ADMIN_SET_FEATURES_CDW10             SetFeatures;
+  NVME_ADMIN_SET_FEATURES_NUM_QUEUES        NumberOfQueuesRequested;
+  NVME_ADMIN_SET_FEATURES_NUM_QUEUES        NumberOfQueuesAllocated;
+
+  Status = EFI_SUCCESS;
+
+  ZeroMem (&CommandPacket, sizeof (EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET));
+  ZeroMem (&Command, sizeof (EFI_NVM_EXPRESS_COMMAND));
+  ZeroMem (&Completion, sizeof (EFI_NVM_EXPRESS_COMPLETION));
+  ZeroMem (&SetFeatures, sizeof (NVME_ADMIN_SET_FEATURES));
+  ZeroMem (&NumberOfQueuesRequested, sizeof (NVME_ADMIN_SET_FEATURES_NUM_QUEUES));
+  ZeroMem (&NumberOfQueuesAllocated, sizeof (NVME_ADMIN_SET_FEATURES_NUM_QUEUES));
+
+  CommandPacket.NvmeCmd        = &Command;
+  CommandPacket.NvmeCompletion = &Completion;
+  CommandPacket.CommandTimeout = NVME_GENERIC_TIMEOUT;
+  CommandPacket.QueueType      = NVME_ADMIN_QUEUE;
+  Command.Nsid                 = 0; // NSID must be set to 0h or FFFFFFFFh for an admin command
+  Command.Cdw0.Opcode          = NVME_ADMIN_SET_FEATURES_CMD;
+
+  // Populate the Set Features Cdw10 and Cdw11 according to Nvm Express 1.3d Spec
+  SetFeatures.Bits.Fid             = NVME_FEATURE_NUMBER_OF_QUEUES;
+  NumberOfQueuesRequested.Bits.NCQ = NCQR;
+  NumberOfQueuesRequested.Bits.NSQ = NSQR;
+  CommandPacket.NvmeCmd->Cdw10     = SetFeatures.Uint32;
+  CommandPacket.NvmeCmd->Cdw11     = NumberOfQueuesRequested.Uint32;
+
+  CommandPacket.NvmeCmd->Flags = CDW10_VALID | CDW11_VALID;
+
+    DEBUG((DEBUG_INFO, "Vnk: Number of Queues Requested: NSQR=%d, NCQR=%d\n", NSQR, NCQR));
+
+  // Send the Set Features Command for Number of Queues
+  Status = Private->Passthru.PassThru (
+                               &Private->Passthru,
+                               0,
+                               &CommandPacket,
+                               NULL
+                               );
+
+  if (EFI_ERROR (Status)) {
+    DEBUG ((DEBUG_ERROR, "Set Features Command for Number of Queues failed with Status %r\n", Status));
+    return Status;
+  }
+
+  //
+  // Save the number of queues allocated, adding 1 to account for it being a 0-based value.
+  // E.g. if 1 pair of data queues is allocated NSQ=0, NCQ=0, then NSQA=1, NCQA=1
+  // These numbers do not include the admin queues.
+  //
+  NumberOfQueuesAllocated.Uint32 = CommandPacket.NvmeCompletion->DW0;
+  Private->NSQA = NumberOfQueuesAllocated.Bits.NSQ + 1;
+  Private->NCQA = NumberOfQueuesAllocated.Bits.NCQ + 1;
+
+  DEBUG((DEBUG_INFO, "Vnk: Number of Queues Allocated: NSQA=%d, NCQA=%d\n", Private->NSQA, Private->NCQA));
+
+  return Status;
+}
+
+/**
+  Create io completion queue(s).
 
   @param  Private          The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
 
@@ -591,7 +672,8 @@ NvmeCreateIoCompletionQueue (
   Status                 = EFI_SUCCESS;
   Private->CreateIoQueue = TRUE;
 
-  for (Index = 1; Index < NVME_MAX_QUEUES; Index++) {
+  // Start from Index 1 because Index 0 is reserved for admin queue
+  for (Index = 1; Index <= Private->NCQA; Index++) {
     ZeroMem (&CommandPacket, sizeof (EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET));
     ZeroMem (&Command, sizeof (EFI_NVM_EXPRESS_COMMAND));
     ZeroMem (&Completion, sizeof (EFI_NVM_EXPRESS_COMPLETION));
@@ -610,11 +692,9 @@ NvmeCreateIoCompletionQueue (
     if (PcdGetBool (PcdSupportAlternativeQueueSize)) {
       QueueSize = MIN (NVME_ALTERNATIVE_MAX_QUEUE_SIZE, Private->Cap.Mqes);
     } else if (Index == 1) {
-      QueueSize = NVME_CCQ_SIZE;
-    } else if (Private->Cap.Mqes > NVME_ASYNC_CCQ_SIZE) {
-      QueueSize = NVME_ASYNC_CCQ_SIZE;
+      QueueSize = MIN (NVME_CCQ_SIZE, Private->Cap.Mqes);
     } else {
-      QueueSize = Private->Cap.Mqes;
+      QueueSize = MIN (NVME_ASYNC_CCQ_SIZE, Private->Cap.Mqes);
     }
 
     // MU_CHANGE [END]
@@ -632,6 +712,7 @@ NvmeCreateIoCompletionQueue (
                                  NULL
                                  );
     if (EFI_ERROR (Status)) {
+      DEBUG ((DEBUG_ERROR, "Create Completion Queue Command %d failed with Status %r\n", Index, Status));
       break;
     }
   }
@@ -642,7 +723,7 @@ NvmeCreateIoCompletionQueue (
 }
 
 /**
-  Create io submission queue.
+  Create io submission queue(s).
 
   @param  Private          The pointer to the NVME_CONTROLLER_PRIVATE_DATA data structure.
 
@@ -666,7 +747,8 @@ NvmeCreateIoSubmissionQueue (
   Status                 = EFI_SUCCESS;
   Private->CreateIoQueue = TRUE;
 
-  for (Index = 1; Index < NVME_MAX_QUEUES; Index++) {
+  // Start from Index 1 because Index 0 is reserved for admin queue
+  for (Index = 1; Index <= Private->NSQA; Index++) {
     ZeroMem (&CommandPacket, sizeof (EFI_NVM_EXPRESS_PASS_THRU_COMMAND_PACKET));
     ZeroMem (&Command, sizeof (EFI_NVM_EXPRESS_COMMAND));
     ZeroMem (&Completion, sizeof (EFI_NVM_EXPRESS_COMPLETION));
@@ -685,11 +767,9 @@ NvmeCreateIoSubmissionQueue (
     if (PcdGetBool (PcdSupportAlternativeQueueSize)) {
       QueueSize = MIN (NVME_ALTERNATIVE_MAX_QUEUE_SIZE, Private->Cap.Mqes);
     } else if (Index == 1) {
-      QueueSize = NVME_CCQ_SIZE;
-    } else if (Private->Cap.Mqes > NVME_ASYNC_CCQ_SIZE) {
-      QueueSize = NVME_ASYNC_CCQ_SIZE;
+      QueueSize = MIN(NVME_CCQ_SIZE, Private->Cap.Mqes);
     } else {
-      QueueSize = Private->Cap.Mqes;
+      QueueSize = MIN(NVME_ASYNC_CCQ_SIZE, Private->Cap.Mqes);
     }
 
     // MU_CHANGE [END]
@@ -709,6 +789,7 @@ NvmeCreateIoSubmissionQueue (
                                  NULL
                                  );
     if (EFI_ERROR (Status)) {
+      DEBUG ((DEBUG_ERROR, "Create Submission Queue Command %d failed with Status %r\n", Index, Status));
       break;
     }
   }
diff --git a/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressHci.h b/MdeModulePkg/Bus/Pci/NvmExpressDxe/NvmExpressHci.h
@@ -66,4 +66,19 @@ NvmeIdentifyNamespace (
   IN VOID                          *Buffer
   );
 
+/**
+ * Request queues from Nvm Express controller .
+ * 
+ * @param Private 
+ * @param NCQR
+ * @param NSQR 
+ * @return EFI_STATUS 
+ */
+EFI_STATUS
+NvmeSetFeaturesNumberOfQueues (
+  IN NVME_CONTROLLER_PRIVATE_DATA  *Private,
+  IN UINT16                        NSQR,
+  IN UINT16                        NCQR
+  );
+
 #endif
