From 8e481f5d91aa66a4f1b9578f8ff9c622400e0d7c Mon Sep 17 00:00:00 2001
From: Matheus <matheusuky@gmail.com>
Date: Thu, 24 Nov 2022 18:43:02 -0300
Subject: [PATCH] pci: legacy_write and pci_setcommand

---
 src/arch/x86_64/include/arch/pci.h |  5 +++++
 src/arch/x86_64/pci.c              | 33 +++++++++++++++++++++++++++++-
 2 files changed, 37 insertions(+), 1 deletion(-)

diff --git a/src/arch/x86_64/include/arch/pci.h b/src/arch/x86_64/include/arch/pci.h
index d55960bd..2dac70f1 100644
--- a/src/arch/x86_64/include/arch/pci.h
+++ b/src/arch/x86_64/include/arch/pci.h
@@ -81,6 +81,11 @@ static inline void* getbarmemaddr(pci_deviceheader* e, int bar){
 		return e->BAR[bar] & ~(0xF);
 }
 
+#define PCI_COMMAND_IO 1
+#define PCI_COMMAND_MEMORY 2
+#define PCI_COMMAND_MASTER 4
+
+void pci_setcommand(pci_enumeration* e, int which, int val);
 pci_enumeration* pci_getdevicecs(int class, int subclass, int n);
 pci_enumeration* pci_getdevicecsp(int class, int subclass, int progif, int n);
 uint64_t pci_msi_build(uint64_t* data, uint8_t vector, uint8_t processor, uint8_t edgetrigger, uint8_t deassert);
diff --git a/src/arch/x86_64/pci.c b/src/arch/x86_64/pci.c
index 5788561a..09f28ea5 100644
--- a/src/arch/x86_64/pci.c
+++ b/src/arch/x86_64/pci.c
@@ -9,6 +9,8 @@ pci_enumeration* enumerations;
 #define CONFIG_ADDRESS 0xCF8
 #define CONFIG_DATA 0xCFC
 
+#define COMMAND_REG 1
+
 pci_enumeration* pci_getdevicecs(int class, int subclass, int n){
 	
 	pci_enumeration* ptr = enumerations;
@@ -73,7 +75,7 @@ pci_enumeration* pci_getdevicecsp(int class, int subclass, int progif, int n){
 	return save;
 }
 
-uint32_t legacy_readdata(int bus, int device, int func, int reg){
+static uint32_t legacy_readdata(int bus, int device, int func, int reg){
 	
 	uint32_t address = reg << 2;
 	address |= (func & 0b111) << 8;
@@ -87,6 +89,35 @@ uint32_t legacy_readdata(int bus, int device, int func, int reg){
 
 }
 
+static void legacy_writedata(int bus, int device, int func, int reg, uint32_t val){
+	
+	uint32_t address = reg << 2;
+        address |= (func & 0b111) << 8;
+        address |= (device & 0b11111) << 11;
+        address |= (bus & 0xFF) << 16;
+        address |= 1 << 31;
+
+        outd(CONFIG_ADDRESS, address);
+	
+	outd(CONFIG_DATA, val);
+
+}
+
+void pci_setcommand(pci_enumeration* e, int which, int val){
+
+	uint32_t regsave = legacy_readdata(e->bus, e->device, e->function, COMMAND_REG);
+
+	if(val == 0){
+		regsave &= ~which;
+	}
+	else{
+		regsave |= which;
+	}
+	
+	legacy_writedata(e->bus, e->device, e->function, COMMAND_REG, regsave);
+
+}
+
 static inline bool legacy_exists(int bus, int device, int func){
 	return legacy_readdata(bus, device, func, 0) != 0xFFFFFFFF;
 }