diff --git a/drivers/nvme/nvme.asm b/drivers/nvme/nvme.asm
index 372d217..17c7b36 100644
--- a/drivers/nvme/nvme.asm
+++ b/drivers/nvme/nvme.asm
@@ -447,11 +447,6 @@ proc device_is_compat stdcall, pci:dword
 		mov 	 dword [esi + pcidev.io_addr], eax
 		mov 	 eax, dword [eax + NVME_MMIO.CAP + 4]
 		and 	 eax, CAP_DSTRD
-
-		; Stride is (2 ^ (2 + DSTRD)) bytes
-		add 	 al, 2
-		stdcall  pow2, eax
-		mov 	 ecx, eax
 		mov 	 byte [esi + pcidev.dstrd], al
 		
 		; 1003h + ((2y + 1) * (4 << CAP.DSTRD))
@@ -582,8 +577,21 @@ proc nvme_init stdcall, pci:dword
 		jz 		.exit_fail
 		mov 	dword [dptr], eax
 		invoke  GetPhysAddr
+		mov 	ebx, eax
+		stdcall get_new_cid, [pci], 0
+		DEBUGF  DBG_INFO, "(NVMe) Got Command Identifier: %u\n", eax
 		;                      pci:dword, nsid:dword, dptr:dword, cid:word, cns:byte
-		stdcall nvme_identify, [pci],     0,          eax,        2,        CNS_IDCS
+		stdcall nvme_identify, [pci],     0,          ebx,        eax,        CNS_IDCS
+		mov 	esi, dword [esi + pcidev.cq_ptr]
+
+; Wait until phase tag bit is set
+@@:
+		mov 	al, byte [esi + CQ_ENTRY.status]
+		test 	al, CQ_PHASE_TAG
+		jz 		@b
+		mov 	esi, dword [dptr]
+		mov 	ax, word [esi + IDENTC.vid]
+		DEBUGF  DBG_INFO, "(NVMe) IDENTC.vid = %x\n", ax
 		
       	xor     eax, eax
       	inc     eax
@@ -598,20 +606,23 @@ proc nvme_init stdcall, pci:dword
 
 endp
 
-proc get_avl_cmd stdcall, pci:dword, y:dword
+proc get_new_cid stdcall, pci:dword, y:dword
 		
 		push 	esi ebx
-		mov 	esi, [pci]
 		mov 	ecx, [y]
-		mov 	eax, dword [esi + ecx * pcidev.queue_entries + NVM_QUEUE_ENTRY.cid_slots]
+		imul 	ecx, sizeof.NVM_QUEUE_ENTRY
+		mov 	esi, [pci]
+		mov 	esi, dword [esi + pcidev.queue_entries]
+		lea 	esi, dword [esi + ecx]
+		mov 	eax, dword [esi + NVM_QUEUE_ENTRY.cid_slots]
 		xor 	edx, edx
 		cmp 	eax, 0xffffffff
 		jne		@f      	
-		mov 	eax, dword [esi + ecx * pcidev.queue_entries + NVM_QUEUE_ENTRY.cid_slots + 4]
+		mov 	eax, dword [esi + NVM_QUEUE_ENTRY.cid_slots + 4]
 		add 	edx, 4
 		cmp 	eax, 0xffffffff
 		jne 	@f
-		pop 	esi
+		pop 	ebx esi
 		mov 	eax, -1
 		ret
 
@@ -626,7 +637,13 @@ proc get_avl_cmd stdcall, pci:dword, y:dword
 		or 		ebx, eax
 
 		mov 	ecx, [y]
-		mov 	dword [esi + ecx * pcidev.queue_entries + NVM_QUEUE_ENTRY.cid_slots + edx], ebx
+		mov 	dword [esi + edx + NVM_QUEUE_ENTRY.cid_slots], ebx
+		bsf 	eax, eax
+		cmp 	edx, 4
+		jne 	@f
+		add 	eax, 32
+
+@@:
 		pop 	ebx esi
 		ret
 
@@ -724,12 +741,14 @@ proc cqyhdbl_write stdcall, pci:dword, y:byte, cqh:word
 		imul 	dx, ax
 		add 	dx, 0x1000
 		movzx 	ecx, [y]
+		imul 	ecx, sizeof.NVM_QUEUE_ENTRY
 		mov 	edi, dword [esi + pcidev.queue_entries]
+		lea 	edi, dword [edi + ecx]
 		mov 	esi, dword [esi + pcidev.io_addr]
 		mov 	ax, [cqh]
 		DEBUGF  DBG_INFO, "(NVMe) Writing to completion queue doorbell register 0x%x: %u\n", dx, ax
 		mov 	word [esi + edx], ax ; Write to CQyHDBL
-		mov 	word [edi + ecx * sizeof.NVM_QUEUE_ENTRY + NVM_QUEUE_ENTRY.head], ax
+		mov 	word [edi + NVM_QUEUE_ENTRY.head], ax
 		pop 	edi esi
 		ret
 
@@ -742,7 +761,6 @@ proc write_admin_cmd stdcall, pci:dword
 		mov 	esi, dword [esi + pcidev.queue_entries]
 		mov 	ax, word [esi + NVM_QUEUE_ENTRY.tail]
 		cmp 	ax, NVM_ASQS
-		jl 		@f
 		xor 	ax, ax
 		
 @@:
@@ -799,8 +817,8 @@ proc irq_handler
 		jmp 	.exit
 
 @@:
-		mov 	dword [esi + NVME_MMIO.INTMC], 0x1
 		stdcall cqyhdbl_write, [p_nvme_devices], 0x0, 0x1
+		mov 	dword [esi + NVME_MMIO.INTMC], 0x1
 
 .exit:
 		; Interrupt handled by driver, return 1
diff --git a/drivers/nvme/nvme.inc b/drivers/nvme/nvme.inc
index be66c3a..28a2eea 100644
--- a/drivers/nvme/nvme.inc
+++ b/drivers/nvme/nvme.inc
@@ -318,8 +318,7 @@ ends
 struct NVM_QUEUE_ENTRY
 	tail 			dw ?
 	head 			dw ?
-	dptr 			dd ?
-	;cid_slots 		dq ?
+	cid_slots 		dq ?
 ends
 
 ; Identify Controller Data Structure