feat: get set_features working and improve IRQ handler

drivers/nvme/nvme.asm

@@ -252,7 +252,7 @@ proc set_features stdcall, pci:dword, dptr:dword, fid:byte, cdw11:dword
 		mov 	eax, [dptr]
 		mov 	dword [esp + SQ_ENTRY.dptr], eax
 		movzx 	eax, [fid]
-		or 		eax, 1 shl 31 ; CDW10.SV
+		;or 		eax, 1 shl 31 ; CDW10.SV
 		mov 	dword [esp + SQ_ENTRY.cdw10], eax
 		mov 	eax, [cdw11]
 		mov 	dword [esp + SQ_ENTRY.cdw11], eax
@@ -520,8 +520,8 @@ proc nvme_init stdcall, pci:dword
 		mov 	dword [edi + NVME_MMIO.ACQ], eax
 		and 	dword [edi + NVME_MMIO.ACQ + 4], 0
 
-		stdcall memset, dword [esi + pcidev.sq_ptr], 0, sizeof.SQ_ENTRY * NVM_ASQS
+		stdcall memset, dword [esi + pcidev.sq_ptr], 0, sizeof.SQ_ENTRY * NVM_CMDS
-		stdcall memset, dword [esi + pcidev.cq_ptr], 0, sizeof.CQ_ENTRY * NVM_ACQS
+		stdcall memset, dword [esi + pcidev.cq_ptr], 0, sizeof.CQ_ENTRY * NVM_CMDS
 
 		; Allocate list of queues
 		invoke  KernelAlloc, sizeof.NVM_QUEUE_ENTRY * NVM_ASQS
@@ -553,12 +553,7 @@ proc nvme_init stdcall, pci:dword
 		invoke  GetPhysAddr
 		;                      pci:dword, nsid:dword, dptr:dword, cns:byte
 		stdcall nvme_identify, [pci],     0,          eax,        CNS_IDCS
-		mov 	ebx, dword [esi + pcidev.cq_ptr]
+		stdcall nvme_cmd_wait, [pci], 0, 0
-
-; Wait until phase tag bit is set
-@@:
-		test 	byte [ebx + CQ_ENTRY.status], CQ_PHASE_TAG
-		jz 		@b
 		mov 	edx, dword [dptr]
 		lea 	edx, byte [edx + IDENTC.sn]
 		lea 	eax, byte [esi + pcidev.serial]
@@ -593,10 +588,20 @@ proc nvme_init stdcall, pci:dword
 		mov 	ebx, eax
 		mov 	eax, (NVM_ASQS - 1) or ((NVM_ACQS - 1) shl 16) ; CDW11 (set the number of queues we want)
 		stdcall set_features, [pci], ebx, 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
+		mov 	esi, dword [esi + pcidev.cq_ptr]
+		mov 	eax, dword [esi + sizeof.CQ_ENTRY + CQ_ENTRY.cdw0]
+		test 	ax, ax
+		jz 		.exit_fail
+		shl 	eax, 16
+		test 	ax, ax
+		jnz		.exit_fail
 		; Creates I/O Queues
 		;stdcall create_io_completion_queue, [pci], 
 		
-		;DEBUGF  DBG_INFO, "(NVMe) Successfully initialized driver!\n"
+		DEBUGF  DBG_INFO, "(NVMe) Successfully initialized driver!\n"
       	xor     eax, eax
       	inc     eax
 		pop 	edi esi ebx
@@ -784,96 +789,61 @@ proc pow2 stdcall, x:byte
 		
 endp
 
+proc nvme_cmd_wait stdcall, pci:dword, y:byte, cid:word
+		
+		push 	esi
+		mov 	esi, [pci]
+		movzx 	ecx, [cid]
+		imul 	ecx, sizeof.CQ_ENTRY
+		mov 	esi, dword [esi + pcidev.cq_ptr]
+
+@@:
+		test	byte [esi + CQ_ENTRY.status], CQ_PHASE_TAG
+		jz 		@b
+		pop 	esi
+		ret
+
+endp
+
 proc irq_handler
 
 		push 	esi edi
 		mov 	esi, dword [p_nvme_devices]
-		mov 	edi, dword [esi + pcidev.cq_ptr]
+		mov 	edi, esi
-		mov 	esi, dword [esi + pcidev.io_addr]
+		mov 	edi, dword [edi + pcidev.io_addr]
-		xor 	ecx, ecx
+		mov 	dword [edi + NVME_MMIO.INTMS], 0x1
-		xor 	edx, edx
+		mov 	esi, dword [esi + pcidev.queue_entries]
-
+		movzx 	ecx, word [esi + NVM_QUEUE_ENTRY.head]
-; Check which completion queue has phase tag bit set
+		mov 	edx, ecx
-@@:
+		imul 	edx, sizeof.CQ_ENTRY
-		mov 	ax, word [edi + ecx + CQ_ENTRY.status]
+		mov 	esi, dword [p_nvme_devices]
-		inc 	dl
+		mov 	esi, dword [esi + pcidev.cq_ptr]
-		add 	ecx, sizeof.CQ_ENTRY
+		mov 	ax, word [esi + edx + CQ_ENTRY.status]
-		test 	ax, CQ_PHASE_TAG
-		jz 		@b
-
-		dec 	dl
-		sub 	ecx, sizeof.CQ_ENTRY
 		and 	ax, not CQ_PHASE_TAG ; ignore phase tag bit
 		DEBUGF  DBG_INFO, "(NVMe) Status: %x\n", ax
 		test 	al, al ; check status code (0 on success)
-		jz 		@f
+		jz 		.ok
-
-		; error occurred
 
+.error:
 		; we have to initiate a controller reset if a admin command encounters
 		; a fatal error or if a completion is not received for a deletion
 	    ; of a submission or completion queue (section 10.1 - page 400 of NVMe 1.4 spec)
-		stdcall nvme_controller_reset, esi
-		stdcall nvme_controller_start, esi
-		jmp 	.cleanup
-
-@@:
-		mov 	esi, dword [p_nvme_devices]
-		mov 	esi, dword [esi + pcidev.queue_entries]
-		imul 	edx, sizeof.NVM_QUEUE_ENTRY
-		mov 	ax, word [esi + edx + NVM_QUEUE_ENTRY.head]
-		mov 	cx, word [edi + ecx + CQ_ENTRY.sqid]
-		DEBUGF  DBG_INFO, "(NVMe) cdw0: 0x%x\n", [edi + ecx + CQ_ENTRY.cdw0]
-		inc 	ax
-		; TODO: Check how many commands were consumed later
-		stdcall cqyhdbl_write, dword [p_nvme_devices], ecx, eax
-
-.cleanup:
-		; Mark this CID as unused in the bitmap
-		xor 	eax, eax
-		inc 	eax
-		mov 	cl, byte [edi + CQ_ENTRY.cid]
-		shl 	eax, cl
-		movzx 	ecx, word [edi + CQ_ENTRY.sqid]
-		imul 	ecx, sizeof.NVM_QUEUE_ENTRY
-		mov 	esi, dword [p_nvme_devices]
-		mov 	esi, dword [esi + pcidev.queue_entries]
-		lea 	esi, dword [esi + ecx]
-		DEBUGF  DBG_INFO, "(NVMe) Freeing CID: %u - 1\n", eax
-		not 	eax
-		cmp 	word [edi + CQ_ENTRY.cid], 32
-		jge 	@f 		 	
-		if __DEBUG__
-			not eax
-			test dword [esi + NVM_QUEUE_ENTRY.cid_slots], eax
-			jnz .not_fail0
-			DEBUGF DBG_INFO, "(NVMe) BUG: CID %u is supposed to be taken\n", cl
-			.fail0:
-			jmp .fail0
-			.not_fail0:
-			not eax
-		end if
-		and 	dword [esi + NVM_QUEUE_ENTRY.cid_slots], eax
-		jmp 	.end
-
-@@:
-		if __DEBUG__
-			not eax
-			test dword [esi + NVM_QUEUE_ENTRY.cid_slots], eax
-			jnz .not_fail1
-			DEBUGF DBG_INFO, "(NVMe) BUG: CID %u is supposed to be taken\n", cl
-			.fail1:
-			jmp .fail0
-			.not_fail1:
-			not eax
-		end if
-		and 	dword [esi + NVM_QUEUE_ENTRY.cid_slots + 4], eax
-
-.end:
-		
 		mov 	esi, dword [p_nvme_devices]
 		mov 	esi, dword [esi + pcidev.io_addr]
-		mov 	dword [esi + NVME_MMIO.INTMC], 0x1
+		stdcall nvme_controller_reset, esi
+		stdcall nvme_controller_start, esi
+		jmp 	.end
+
+.ok:
+		mov 	eax, dword [esi + edx + CQ_ENTRY.cdw0]
+		inc 	ecx
+		; TODO: Check how many commands were consumed later
+		stdcall cqyhdbl_write, dword [p_nvme_devices], 0, ecx
+
+.end:
+		mov 	edi, dword [p_nvme_devices]
+		mov 	edi, dword [edi + pcidev.io_addr]
+		mov 	dword [edi + NVME_MMIO.INTMC], 0x1
 
 		; Interrupt handled by driver, return 1
 		xor 	eax, eax