From 33a774d1cf68d7b79669329964d98990446b176d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Markus=20Fr=C3=B6schle?= <mfro@mubf.de>
Date: Mon, 30 Dec 2013 09:58:14 +0000
Subject: [PATCH] implemented pci_find_classcode()

---
 BaS_gcc/include/pci.h | 19 ++++++-----
 BaS_gcc/pci/pci.c     | 78 +++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 89 insertions(+), 8 deletions(-)

diff --git a/BaS_gcc/include/pci.h b/BaS_gcc/include/pci.h
index 6ae8f80..a3832ce 100644
--- a/BaS_gcc/include/pci.h
+++ b/BaS_gcc/include/pci.h
@@ -191,16 +191,16 @@ typedef struct                       /* structure of address conversion     */
 #define PCI_COMMAND(i)		(((i) >> 16) & 0xffff)
 
 /* register 0x08 macros */
-#define PCI_CLASS_CODE(i)	((swpl((i)) & 0xffff0000) >> 16)
-#define PCI_SUBCLASS(i)		((swpl((i)) & 0xffffff00) >> 8)
-#define PCI_PROF_IF(i)		(((i) & 0xff00) >> 8)
-#define PCI_REVISION_ID(i)	(((i) & 0xff))
+#define PCI_CLASS_CODE(i)	((swpl((i)) & 0xff000000) >> 24)
+#define PCI_SUBCLASS(i)		((swpl((i)) & 0x00ff0000) >> 16)
+#define PCI_PROG_IF(i)		((swpl((i)) & 0x0000ff00) >> 8)
+#define PCI_REVISION_ID(i)	((swpl((i)) & 0x000000ff))
 
 /* register 0x0c macros */
-#define PCI_BIST(i)			(((i) & 0xff000000) >> 24)
-#define PCI_HEADER_TYPE(i)	(((i) & 0xff0000) >> 16)
-#define PCI_LAT_TIMER(i)	(((i) & 0xff00) >> 8)
-#define PCI_CACHELINE_SIZE(i)	(((i) & 0xff))
+#define PCI_BIST(i)			((swpl((i)) & 0xff000000) >> 24)
+#define PCI_HEADER_TYPE(i)	((swpl((i)) & 0x00ff0000) >> 16)
+#define PCI_LAT_TIMER(i)	((swpl((i)) & 0x0000ff00) >> 8)
+#define PCI_CACHELINE_SIZE(i)	((swpl((i)) & 0x000000ff))
 
 /* register 0x2c macros */
 #define PCI_SUBSYS_ID(i)	(((i) & 0xffff0000) >> 16)
@@ -220,11 +220,14 @@ typedef struct                       /* structure of address conversion     */
 #define PCI_MEMBAR_TYPE(i)	(((i) & 0x6) >> 1)
 #define PCI_IOBAR_ADR(i)	(((i) & 0xfffffffc))
 #define PCI_MEMBAR_ADR(i)	(((i) & 0xfffffff0))
+
 extern void init_eport(void);
 extern void init_xlbus_arbiter(void);
 extern void init_pci(void);
 
 extern int32_t pci_find_device(uint16_t device_id, uint16_t vendor_id, int index);
+extern int32_t pci_find_classcode(uint32_t classcode, int index);
+
 
 extern uint32_t pci_read_config_longword(int32_t handle, int offset);
 extern uint16_t pci_read_config_word(int32_t handle, int offset);
diff --git a/BaS_gcc/pci/pci.c b/BaS_gcc/pci/pci.c
index a09615a..c6d9707 100644
--- a/BaS_gcc/pci/pci.c
+++ b/BaS_gcc/pci/pci.c
@@ -498,6 +498,84 @@ int32_t pci_find_device(uint16_t device_id, uint16_t vendor_id, int index)
 	return PCI_DEVICE_NOT_FOUND;
 }
 
+/*
+ * pci_find_classcode(uint32_t classcode, int index)
+ *
+ * Find the index'th pci device with a specific classcode. Bits 0-23 describe this classcode.
+ * Bits 24 - 26 describe what needs to match: 24: prog interface, 25: PCI subclass, 26: PCI base class.
+ * If no bits are set, there is a match for each device.
+ */
+int32_t pci_find_classcode(uint32_t classcode, int index)
+{
+	uint16_t bus;
+	uint16_t device;
+	uint16_t function = 0;
+	uint16_t n = 0;
+	int32_t handle;
+
+	for (bus = 0; bus < 2; bus++)
+	{
+		for (device = 10; device < 31; device++)
+		{
+			uint32_t value;
+			uint8_t htr;
+
+			handle = PCI_HANDLE(bus, device, 0);
+
+			value = pci_read_config_longword(handle, PCIIDR);
+			
+			if (value != 0xffffffff)	/* device found */
+			{
+				value = pci_read_config_longword(handle, PCICCR);
+			
+				if ((classcode & (1 << 26) ? ((PCI_CLASS_CODE(value) == (classcode & 0xff))) : true) &&
+					(classcode & (1 << 25) ? ((PCI_SUBCLASS(value) == ((classcode & 0xff00) >> 8))) : true) &&
+					(classcode & (1 << 24) ? ((PCI_PROG_IF(value) == ((classcode & 0xff0000) >> 16))) : true))
+				{
+					if (n == index)
+					{
+						return handle;
+					}
+					n++;
+				}
+
+				/*
+			 	* there is a device at this position, but not the one we are looking for.
+			 	* Check to see if it is a multi-function device. We need to look "behind" it
+			 	* for the other functions in that case.
+			 	*/
+				if ((htr = pci_read_config_byte(handle, PCIHTR)) & 0x80)
+				{
+					/* yes, this is a multi-function device, look for more functions */
+
+					for (function = 1; function < 8; function++)
+					{
+						handle = PCI_HANDLE(bus, device, function);
+						value = pci_read_config_longword(handle, PCIIDR);
+
+						if (value != 0xffffffff)	/* device found */
+						{
+							value = pci_read_config_longword(handle, PCICCR);
+
+							if ((classcode & (1 << 26) ? ((PCI_CLASS_CODE(value) == (classcode & 0xff))) : true) &&
+								(classcode & (1 << 25) ? ((PCI_SUBCLASS(value) == ((classcode & 0xff00) >> 8))) : true) &&
+								(classcode & (1 << 24) ? ((PCI_PROG_IF(value) == ((classcode & 0xff0000) >> 16))) : true))
+							{
+								if (n == index)
+								{
+									return handle;
+								}
+								n++;
+							}
+						}
+					}
+				}
+			}
+		}
+	}
+	return PCI_DEVICE_NOT_FOUND;
+}
+
 int32_t pci_hook_interrupt(int32_t handle, void *handler, void *parameter)
 {
 	/* FIXME: implement */