tidyup IRQ handler, plus some other fixes

drivers/nvme/nvme.asm

@@ -282,6 +282,7 @@ proc determine_active_nsids stdcall, pci:dword
 		test 	eax, eax
 		jz 		.not_active_namespace
 		mov 	ebx, ecx
+		jmp 	.ret
 
 .not_active_namespace:
 		inc 	ecx
@@ -732,7 +733,7 @@ proc nvme_init stdcall, pci:dword
 		jl 		.exit_fail
 		invoke  KernelFree, edi
 
-		mov 	eax, (NVM_ASQS - 1) or ((NVM_ACQS - 1) shl 16) ; CDW11 (set the number of queues we want)
+		mov 	eax, 1 or (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]
 		mov 	esi, dword [esi + pcidev.queue_entries]
@@ -894,7 +895,7 @@ proc nvme_wait stdcall, mmio:dword
 endp
 
 ; Writes to completion queue 'y' head doorbell
-proc cqyhdbl_write stdcall, pci:dword, y:dword, cqh:word
+proc cqyhdbl_write stdcall, pci:dword, y:dword, cqh:dword
 
 		push 	esi edi
 		mov 	esi, [pci]
@@ -913,7 +914,7 @@ proc cqyhdbl_write stdcall, pci:dword, y:dword, cqh:word
 		mov 	edi, dword [esi + pcidev.queue_entries]
 		lea 	edi, dword [edi + ecx]
 		mov 	esi, dword [esi + pcidev.io_addr]
-		mov 	ax, [cqh]
+		mov 	eax, [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 + NVM_QUEUE_ENTRY.head], ax
@@ -1047,7 +1048,37 @@ proc is_queue_full stdcall, tail:word, head:word
 
 endp
 
-;proc consume_cq_entries stdcall, 
+proc consume_cq_entries stdcall, pci:dword, queue:dword
+
+		push 	esi edi
+		mov 	esi, [pci]
+		mov 	ecx, [queue]
+		imul 	ecx, sizeof.NVM_QUEUE_ENTRY
+		mov 	esi, dword [esi + pcidev.queue_entries]
+		lea 	esi, [esi + ecx]
+		mov 	edi, dword [esi + NVM_QUEUE_ENTRY.cq_ptr]
+		movzx 	eax, word [esi + NVM_QUEUE_ENTRY.tail]
+		movzx 	ecx, word [esi + NVM_QUEUE_ENTRY.head]
+		stdcall is_queue_full, eax, ecx
+		test 	eax, eax
+		jnz 	.end
+		movzx 	ecx, word [esi + NVM_QUEUE_ENTRY.head]
+		
+.loop:
+		cmp 	cx, word [esi + NVM_QUEUE_ENTRY.tail]
+		je 		.end
+		inc 	cx
+		push 	ecx
+		stdcall cqyhdbl_write, [pci], [queue], ecx
+		pop 	ecx
+		jmp 	.loop
+
+.end:
+		pop 	edi esi
+		xor 	eax, eax
+		ret
+
+endp
 
 proc irq_handler
 
@@ -1062,61 +1093,13 @@ proc irq_handler
 		mov 	edi, esi
 		mov 	edi, dword [edi + pcidev.io_addr]
 		mov 	dword [edi + NVME_MMIO.INTMS], 0x1
-		mov 	esi, dword [esi + pcidev.queue_entries]
-		movzx 	ecx, word [esi + NVM_QUEUE_ENTRY.head]
-		movzx 	edx, word [esi + NVM_QUEUE_ENTRY.tail]
-		;DEBUGF  DBG_INFO, "IRQ (head): 0x%x, (tail): 0x%x\n", cx, dx
-		stdcall is_queue_full, edx, ecx
-		test 	eax, eax
-		jnz 	.end
-		mov 	edx, ecx
-		imul 	edx, sizeof.CQ_ENTRY
-		mov 	esi, dword [p_nvme_devices]
-		mov 	esi, dword [esi + pcidev.queue_entries]
-		mov 	esi, dword [esi + NVM_QUEUE_ENTRY.cq_ptr]
-		mov 	ax, word [esi + edx + CQ_ENTRY.status]
-		and 	ax, not CQ_PHASE_TAG ; ignore phase tag bit
-		;DEBUGF  DBG_INFO, "(NVMe) Status: 0x%x\n", ax
-		test 	al, al ; check status code (0 on success)
-		jz 		.ok
-
-.error:
-		jmp 	@b
-		; 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)
-		;mov 	esi, dword [p_nvme_devices]
-		;stdcall nvme_controller_reset, esi
-		;stdcall nvme_controller_start, esi
-		;jmp 	.end
-
-.ok:
-		mov 	eax, dword [esi + edx + CQ_ENTRY.cdw0]
-		inc 	ecx
-		cmp 	ecx, NVM_ACQS
-		jng 	@f
-		mov 	esi, dword [p_nvme_devices]
-		mov 	esi, dword [esi + pcidev.queue_entries]
-		mov 	eax, dword [esi + NVM_QUEUE_ENTRY.phase_tag]
-		not 	eax
-		and 	eax, 0x1
-		mov 	dword [esi + NVM_QUEUE_ENTRY.phase_tag], eax
-		xor 	ecx, ecx
-		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]
+		stdcall consume_cq_entries, [p_nvme_devices], 0
 		mov 	dword [edi + NVME_MMIO.INTMC], 0x1
 
 		; Interrupt handled by driver, return 1
+		pop 	edi esi
 		xor 	eax, eax
 		inc 	eax
-		pop 	edi esi
 		ret
 
 .not_our_irq: