diff --git a/drivers/nvme/nvme.asm b/drivers/nvme/nvme.asm
index 1c331b2..76daf94 100644
--- a/drivers/nvme/nvme.asm
+++ b/drivers/nvme/nvme.asm
@@ -566,15 +566,16 @@ proc nvme_init stdcall, pci:dword
 		mov 	eax, (NVM_ASQS - 1) or ((NVM_ACQS - 1) shl 16) ; CDW11 (set the number of queues we want)
 		stdcall set_features, [pci], NULLPTR, FID_NUMBER_OF_QUEUES, eax
 		mov 	esi, dword [p_nvme_devices]
-		stdcall nvme_wait, dword [esi + pcidev.io_addr]
-		;stdcall nvme_cmd_wait, [pci], 0, 1
+		;stdcall nvme_wait, dword [esi + pcidev.io_addr]
+		stdcall nvme_cmd_wait, [pci], 0, 1
 		mov 	esi, dword [esi + pcidev.queue_entries]
 		mov 	esi, dword [esi + NVM_QUEUE_ENTRY.cq_ptr]
 		mov 	eax, dword [esi + sizeof.CQ_ENTRY + CQ_ENTRY.cdw0]
-		test 	ax, ax
+		DEBUGF  DBG_INFO, "(NVMe) Set Features CDW0: 0x%x\n", eax
+		test 	ax, ax ; Number of I/O Submission Queues allocated
 		jz 		.exit_fail
 		shl 	eax, 16
-		test 	ax, ax
+		test 	ax, ax ; Number of I/O Completion Queues allocated
 		jnz		.exit_fail
 
 		; Create I/O Queues
@@ -786,13 +787,16 @@ proc nvme_cmd_wait stdcall, pci:dword, y:byte, cid:word
 		push 	esi
 		mov 	esi, [pci]
 		movzx 	ecx, [cid]
-		imul 	ecx, sizeof.CQ_ENTRY
+		movzx 	edx, [y]
+		imul 	edx, sizeof.NVM_QUEUE_ENTRY
 		mov 	esi, dword [esi + pcidev.queue_entries]
+		lea 	esi, [esi + edx]
+		imul 	ecx, sizeof.CQ_ENTRY
 		mov 	esi, dword [esi + NVM_QUEUE_ENTRY.cq_ptr]
 
 ; WARNING: status bit flips on each pass, count for this later...
 @@:
-		test	byte [esi + CQ_ENTRY.status], CQ_PHASE_TAG
+		test	byte [esi + ecx + CQ_ENTRY.status], CQ_PHASE_TAG
 		jz 		@b
 		pop 	esi
 		ret