From a0cc2ec6a58974a38b9ed0bdebb01e98995fb7f9 Mon Sep 17 00:00:00 2001
From: Abdur-Rahman Mansoor <armansoor@missingno.dev>
Date: Thu, 30 May 2024 17:11:24 -0400
Subject: [PATCH] add `sqytdbl_write` and `cqyhdbl_write`

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

diff --git a/drivers/nvme/nvme.asm b/drivers/nvme/nvme.asm
index 068d186..79dd0c3 100644
--- a/drivers/nvme/nvme.asm
+++ b/drivers/nvme/nvme.asm
@@ -337,7 +337,7 @@ proc nvme_init stdcall, pci:dword
 
 		; Attach interrupt handler
 		movzx 	eax, byte [esi + pcidev.iline]
-		invoke  AttachIntHandler, eax, irq_handler, 0
+		invoke  AttachIntHandler, eax, irq_handler, esi
 		test 	eax, eax
 		jz 		.exit_fail
 		DEBUGF  DBG_INFO, "(NVMe) Successfully attached interrupt handler\n"
@@ -411,14 +411,31 @@ proc nvme_wait stdcall, mmio:dword
 
 endp
 
-proc cq0thbl_write stdcall, mmio:dword, dstrd:dword
+; Writes to completion queue 'y' head doorbell
+proc cqyhdbl_write stdcall, mmio:dword, dstrd:byte, y:byte
 
-		; 1000h + (2 * (4 << CAP.DSTRD))
-		mov 	eax, [dstrd]
-		shl 	eax, 4
-		imul 	eax, 2
+		; 1000h + ((2y + 1) * (4 << CAP.DSTRD))
+		xor 	ecx, ecx
+		shl 	ecx, [y]
+		inc 	ecx
+		mov 	eax, 4
+		shl 	eax, [dstrd]
+		imul 	eax, ecx
 		add 	eax, 0x1000
+		ret
 
+endp
+
+; Writes to submission queue 'y' tail doorbell
+proc sqytdbl_write stdcall, mmio:dword, dstrd:byte, y:byte
+
+		; 1000h + (2y * (4 << CAP.DSTRD))
+		xor 	ecx, ecx
+		shl 	ecx, [y]
+		mov 	eax, 4
+		shl 	eax, [dstrd]
+		imul 	eax, ecx
+		add 	eax, 0x1000
 		ret
 
 endp