From 8557d0a450e22cedb5dcc144a117221bc7b72ab1 Mon Sep 17 00:00:00 2001
From: Abdur-Rahman Mansoor <armansoor@missingno.dev>
Date: Mon, 5 Aug 2024 12:49:28 -0400
Subject: [PATCH] tidy up queue allocation

---
 drivers/nvme/nvme.asm | 43 ++++++++++++++++++++-----------------------
 1 file changed, 20 insertions(+), 23 deletions(-)

diff --git a/drivers/nvme/nvme.asm b/drivers/nvme/nvme.asm
index 420d891..39f19e8 100644
--- a/drivers/nvme/nvme.asm
+++ b/drivers/nvme/nvme.asm
@@ -664,10 +664,16 @@ proc nvme_init stdcall, pci:dword
 	end if
 
 	; check maximum queue entries supported
-	mov 	ax, word [edi + NVME_MMIO.CAP]
+	mov 	eax, dword [edi + NVME_MMIO.CAP]
 	cmp 	ax, SQ_ENTRIES
 	jb 	.exit_fail
 	DEBUGF  DBG_INFO, "nvme%u: OK... maximum queue entries is %u\n", [esi + pcidev.num], ax
+
+	if __DEBUG__
+		test 	eax, CAP_CQR
+		setnz 	al
+		DEBUGF  DBG_INFO, "nvme%u: Contiguous queues required: %u\n", [esi + pcidev.num], al
+	end if
 	
 	; For some reason, bit 7 (No I/O command set supported) is also set to 1 despite bit 0 (NVM command set)
 	; being set to 1.. so I am not sure if bit 7 should be checked at all.. investigate later.
@@ -721,34 +727,25 @@ proc nvme_init stdcall, pci:dword
 	stdcall memsetdz, eax, sizeof.NVM_QUEUE_ENTRY * (LAST_QUEUE_ID + 1) / 4
 
 	; Allocate submission/completion queue pointers
-	; TODO: Make these queues physically contiguous
-	xor 	ecx, ecx
+	xor 	ebx, ebx
 
 .init_queues:
-	push 	ecx
-	invoke  CreateRingBuffer, 0x1000, PG_SW
-	pop 	ecx
+	invoke  KernelAlloc, PAGE_SIZE * 2
 	test 	eax, eax
 	jz 	.exit_fail
-	mov 	dword [edi + ecx + NVM_QUEUE_ENTRY.sq_ptr], eax
-	push 	ecx
-	stdcall memsetdz, eax, sizeof.SQ_ENTRY * SQ_ENTRIES / 4
-	invoke  CreateRingBuffer, 0x1000, PG_SW
-	pop 	ecx
-	test 	eax, eax
-	jz 	.exit_fail
-	mov 	dword [edi + ecx + NVM_QUEUE_ENTRY.cq_ptr], eax
-	push 	ecx
-	stdcall memsetdz, eax, sizeof.CQ_ENTRY * CQ_ENTRIES / 4
+	mov 	dword [edi + ebx + NVM_QUEUE_ENTRY.sq_ptr], eax
+	add 	eax, PAGE_SIZE
+	mov 	dword [edi + ebx + NVM_QUEUE_ENTRY.cq_ptr], eax
+	sub 	eax, PAGE_SIZE
+	stdcall memsetdz, eax, PAGE_SIZE * 2 / 4
 
 	; Initialize command entries
 	invoke  KernelAlloc, sizeof.NVMQCMD * CQ_ENTRIES
-	pop 	ecx
 	test 	eax, eax
 	jz 	.exit_fail
-	mov 	dword [edi + ecx + NVM_QUEUE_ENTRY.cmd_ptr], eax
+	mov 	dword [edi + ebx + NVM_QUEUE_ENTRY.cmd_ptr], eax
 	mov  	esi, eax
-	push 	ecx
+	push 	ebx
 	xor 	ebx, ebx
 
 .init_cmd_entries:
@@ -765,9 +762,9 @@ proc nvme_init stdcall, pci:dword
 	cmp 	ebx, CQ_ENTRIES
 	jne 	.init_cmd_entries
 
-	pop 	ecx
-	add 	ecx, sizeof.NVM_QUEUE_ENTRY
-	cmp 	ecx, (LAST_QUEUE_ID + 1) * sizeof.NVM_QUEUE_ENTRY
+	pop 	ebx
+	add 	ebx, sizeof.NVM_QUEUE_ENTRY
+	cmp 	ebx, (LAST_QUEUE_ID + 1) * sizeof.NVM_QUEUE_ENTRY
 	jne 	.init_queues
 
 	; Configure Admin Submission/Completion Queue Base Address
@@ -1126,7 +1123,7 @@ proc sqytdbl_write stdcall, pci:dword, y:word, cmd:dword
 	shl 	edx, cl
 	imul 	edx, ebx
 	add 	edx, 0x1000
-	DEBUGF  DBG_INFO, "nvme%u: Writing to SQ%u doorbell register 0x%x: %u\n", [esi + pcidev.num], [y], dx, ax
+	;DEBUGF  DBG_INFO, "nvme%u: Writing to SQ%u doorbell register 0x%x: %u\n", [esi + pcidev.num], [y], dx, ax
 	mov 	word [edi + NVM_QUEUE_ENTRY.tail], ax
 	mov 	esi, dword [esi + pcidev.io_addr]
 	mov 	word [esi + edx], ax