refactor

drivers/nvme/nvme.asm

@@ -98,14 +98,13 @@ endp
 ; See pages 161-205 of the NVMe 1.4 specification for reference
 proc nvme_identify stdcall, pci:dword, nsid:dword, dptr:dword, cns:byte
 
-		push 	esi
-		mov 	esi, [pci]
-
+		sub 	esp, sizeof.SQ_ENTRY
 		; It's important to check if CNS is a valid value here. In revision 1.0
 		; CNS is a 1 bit field and a two bit field in revision 1.1, using invalid
 		; values results in undefined behavior (see page 162 of NVMe 1.4 spec)
 		if __DEBUG__
 			push 	esi
+			mov 	esi, [pci]
 			mov 	esi, dword [esi + pcidev.io_addr]
 			mov 	eax, dword [esi + NVME_MMIO.VS]
 			cmp 	eax, VS110
@@ -131,20 +130,19 @@ proc nvme_identify stdcall, pci:dword, nsid:dword, dptr:dword, cns:byte
 			
 		end if
 
-		mov 	esi, [esi + pcidev.sq_ptr]
-		stdcall memset, esi, 0, sizeof.SQ_ENTRY
+		stdcall memset, esp, 0, sizeof.SQ_ENTRY
 
 		mov     eax, [nsid]
-		mov     dword [esi + SQ_ENTRY.nsid], eax
+		mov     dword [esp + SQ_ENTRY.nsid], eax
 		mov     eax, [dptr]
-		mov     dword [esi + SQ_ENTRY.dptr], eax
+		mov     dword [esp + SQ_ENTRY.dptr], eax
 		stdcall set_cdw0, [pci], ADMIN_QUEUE, ADM_CMD_IDENTIFY
-		mov 	dword [esi + SQ_ENTRY.cdw0], eax
+		mov 	dword [esp + SQ_ENTRY.cdw0], eax
 		mov     al, [cns]
-		mov     byte [esi + SQ_ENTRY.cdw10], al
-		stdcall write_admin_cmd, [pci]
+		mov     byte [esp + SQ_ENTRY.cdw10], al
+		stdcall write_admin_cmd, [pci], esp
 
-		pop 	esi
+		add 	esp, sizeof.SQ_ENTRY
 		ret
 
 endp
@@ -245,21 +243,21 @@ endp
 
 
 ; See page 205 of the NVMe 1.4 specification for reference
-proc set_features stdcall, pci:dword, dptr:dword, fid:byte
+proc set_features stdcall, pci:dword, dptr:dword, fid:byte, cdw11:dword
 		
-		push 	esi
-		mov 	esi, [pci]
-		mov 	esi, dword [esi + pcidev.sq_ptr]
-		stdcall memset, esi, 0, sizeof.SQ_ENTRY
+		sub 	esp, sizeof.SQ_ENTRY
+		stdcall memset, esp, 0, sizeof.SQ_ENTRY
 		stdcall set_cdw0, [pci], ADMIN_QUEUE, ADM_CMD_SET_FEATURES
-		mov 	dword [esi + SQ_ENTRY.cdw0], eax
+		mov 	dword [esp + SQ_ENTRY.cdw0], eax
 		mov 	eax, [dptr]
-		mov 	dword [esi + SQ_ENTRY.dptr], eax
+		mov 	dword [esp + SQ_ENTRY.dptr], eax
 		movzx 	eax, [fid]
 		or 		eax, 1 shl 31 ; CDW10.SV
-		mov 	dword [esi + SQ_ENTRY.cdw10], eax
-		stdcall write_admin_cmd, [pci]
-		pop 	esi
+		mov 	dword [esp + SQ_ENTRY.cdw10], eax
+		mov 	eax, [cdw11]
+		mov 	dword [esp + SQ_ENTRY.cdw11], eax
+		stdcall write_admin_cmd, [pci], esp
+		add 	esp, sizeof.SQ_ENTRY
 		ret
 
 endp
@@ -592,7 +590,9 @@ proc nvme_init stdcall, pci:dword
 		jl 		.exit_fail
 
 		invoke  GetPhysAddr, esi
-		stdcall set_features, [pci], eax, FID_NUMBER_OF_QUEUES
+		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
 		; Creates I/O Queues
 		;stdcall create_io_completion_queue, [pci], 
 		
@@ -705,7 +705,7 @@ proc nvme_wait stdcall, mmio:dword
 endp
 
 ; Writes to completion queue 'y' head doorbell
-proc cqyhdbl_write stdcall, pci:dword, y:byte, cqh:word
+proc cqyhdbl_write stdcall, pci:dword, y:word, cqh:word
 
 		push 	esi edi
 		mov 	esi, [pci]
@@ -733,23 +733,34 @@ proc cqyhdbl_write stdcall, pci:dword, y:byte, cqh:word
 
 endp
 
-proc write_admin_cmd stdcall, pci:dword
+proc write_admin_cmd stdcall, pci:dword, cmd:dword
 
-		push 	esi
-		mov 	esi, [pci]
+		push 	esi edi
+		mov 	edi, [pci]
+		mov 	esi, [cmd]
+		mov 	ecx, dword [esi + SQ_ENTRY.cdw0]
+		shr 	ecx, 16 ; Get CID
+		imul 	ecx, sizeof.SQ_ENTRY
+		mov 	edi, dword [edi + pcidev.sq_ptr]
+		lea 	edi, [edi + ecx]
+		stdcall memcpy, edi, esi, sizeof.SQ_ENTRY
+		
+		mov 	esi, dword [edi + pcidev.sq_ptr]
 		mov 	esi, dword [esi + pcidev.queue_entries]
+		mov 	edi, esi
 		mov 	ax, word [esi + NVM_QUEUE_ENTRY.tail]
 		cmp 	ax, NVM_ASQS
-		je		@f 		
+		jng		@f 		
 		xor 	ax, ax
 		
 @@:
 		mov 	esi, [pci]
 		mov 	esi, dword [esi + pcidev.io_addr]
 		inc 	ax
+		DEBUGF  DBG_INFO, "(NVMe) Writing to Admin Submission Queue: %u\n", ax
 		mov 	word [esi + 0x1000], ax
-		mov 	word [esi + NVM_QUEUE_ENTRY.tail], ax
-		pop 	esi
+		mov 	word [edi + NVM_QUEUE_ENTRY.tail], ax
+		pop 	edi esi
 		ret
 
 endp
@@ -777,11 +788,21 @@ proc irq_handler
 
 		push 	esi edi
 		mov 	esi, dword [p_nvme_devices]
-		mov 	edi, esi
+		mov 	edi, dword [esi + pcidev.cq_ptr]
 		mov 	esi, dword [esi + pcidev.io_addr]
-		mov 	edi, dword [edi + pcidev.cq_ptr]
-		mov 	dword [esi + NVME_MMIO.INTMS], 0x1
-		mov 	ax, word [edi + CQ_ENTRY.status]
+		xor 	ecx, ecx
+		xor 	edx, edx
+
+; Check which completion queue has phase tag bit set
+@@:
+		mov 	ax, word [edi + ecx + CQ_ENTRY.status]
+		inc 	dl
+		add 	ecx, sizeof.CQ_ENTRY
+		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)
@@ -797,7 +818,15 @@ proc irq_handler
 		jmp 	.cleanup
 
 @@:
-		stdcall cqyhdbl_write, [p_nvme_devices], 0x0, 0x1
+		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