From 1d58cb8832bd8ab7dd6e0fb94f32cfd0f145c8a4 Mon Sep 17 00:00:00 2001
From: cinap_lenrek <cinap_lenrek@rei2.9hal>
Date: Sat, 11 Feb 2012 00:49:39 +0100
Subject: [PATCH] pci: add pcicap

---
 sys/src/9/pc/fns.h |  2 +-
 sys/src/9/pc/io.h  | 17 ++++++++++
 sys/src/9/pc/mp.c  | 12 ++-----
 sys/src/9/pc/pci.c | 81 +++++++++++++++++++++-------------------------
 4 files changed, 58 insertions(+), 54 deletions(-)

diff --git a/sys/src/9/pc/fns.h b/sys/src/9/pc/fns.h
index e6531b0d6..0c1774a32 100644
--- a/sys/src/9/pc/fns.h
+++ b/sys/src/9/pc/fns.h
@@ -128,7 +128,7 @@ void	pcihinv(Pcidev*);
 uchar	pciipin(Pcidev*, uchar);
 Pcidev* pcimatch(Pcidev*, int, int);
 Pcidev* pcimatchtbdf(int);
-int	pcinextcap(Pcidev*, int);
+int	pcicap(Pcidev*, int);
 void	pcireset(void);
 int	pciscan(int, Pcidev**);
 void	pcisetbme(Pcidev*);
diff --git a/sys/src/9/pc/io.h b/sys/src/9/pc/io.h
index 197a91799..95d7d55eb 100644
--- a/sys/src/9/pc/io.h
+++ b/sys/src/9/pc/io.h
@@ -216,6 +216,23 @@ enum {					/* type 2 pre-defined header */
 	PciCBLMBAR	= 0x44,		/* legacy mode base address */
 };
 
+/* capabilities */
+enum {
+        PciCapPMG       = 0x01,         /* power management */
+        PciCapAGP       = 0x02,
+        PciCapVPD       = 0x03,         /* vital product data */
+        PciCapSID       = 0x04,         /* slot id */
+        PciCapMSI       = 0x05,
+        PciCapCHS       = 0x06,         /* compact pci hot swap */
+        PciCapPCIX      = 0x07,
+        PciCapHTC       = 0x08,         /* hypertransport irq conf */
+        PciCapVND       = 0x09,         /* vendor specific information */
+        PciCapPCIe      = 0x10,
+        PciCapMSIX      = 0x11,
+        PciCapSATA      = 0x12,
+        PciCapHSW       = 0x0c,         /* hot swap */
+};
+
 typedef struct Pcisiz Pcisiz;
 struct Pcisiz
 {
diff --git a/sys/src/9/pc/mp.c b/sys/src/9/pc/mp.c
index a83cb1913..74bc9c62f 100644
--- a/sys/src/9/pc/mp.c
+++ b/sys/src/9/pc/mp.c
@@ -906,15 +906,9 @@ msiintrenable(Vctl *v)
 		print("msiintrenable: could not find Pcidev for tbdf %.8x\n", tbdf);
 		return -1;
 	}
-	cap = 0;
-	for(;;) {
-		cap = pcinextcap(pci, cap);
-		if(cap == 0)
-			return -1;
-		if(pcicfgr8(pci, cap) == 0x05) /* MSI block */
-			break;
-	}
-	
+	cap = pcicap(pci, PciCapMSI);
+	if(cap < 0)
+		return -1;
 	vno = allocvector();
 	cpu = mpintrcpu();
 	ok64 = (pcicfgr16(pci, cap + MSICtrl) & (1<<7)) != 0;
diff --git a/sys/src/9/pc/pci.c b/sys/src/9/pc/pci.c
index decad75ba..f36f01baf 100644
--- a/sys/src/9/pc/pci.c
+++ b/sys/src/9/pc/pci.c
@@ -1402,53 +1402,46 @@ pciclrmwi(Pcidev* p)
 	pcicfgw16(p, PciPCR, p->pcr);
 }
 
+int
+pcicap(Pcidev *p, int cap)
+{
+        int i, c, off;
+
+        /* status register bit 4 has capabilities */
+        if((pcicfgr16(p, PciPSR) & 1<<4) == 0)
+                return -1;      
+        switch(pcicfgr8(p, PciHDT) & 0x7F){
+        default:
+                return -1;
+        case 0:                         /* etc */
+        case 1:                         /* pci to pci bridge */
+                off = 0x34;
+                break;
+        case 2:                         /* cardbus bridge */
+                off = 0x14;
+                break;
+        }
+        for(i = 48; i--;){
+                off = pcicfgr8(p, off);
+                if(off < 0x40 || (off & 3))
+                        break;
+                off &= ~3;
+                c = pcicfgr8(p, off);
+                if(c == 0xff)
+                        break;
+                if(c == cap)
+                        return off;
+                off++;
+        }
+        return -1;
+}
+
 static int
 pcigetpmrb(Pcidev* p)
 {
-	int ptr;
-
-	if(p->pmrb != 0)
-		return p->pmrb;
-	p->pmrb = -1;
-
-	/*
-	 * If there are no extended capabilities implemented,
-	 * (bit 4 in the status register) assume there's no standard
-	 * power management method.
-	 * Find the capabilities pointer based on PCI header type.
-	 */
-	if(!(pcicfgr16(p, PciPSR) & 0x0010))
-		return -1;
-	switch(pcicfgr8(p, PciHDT)){
-	default:
-		return -1;
-	case 0:					/* all other */
-	case 1:					/* PCI to PCI bridge */
-		ptr = 0x34;
-		break;
-	case 2:					/* CardBus bridge */
-		ptr = 0x14;
-		break;
-	}
-	ptr = pcicfgr32(p, ptr);
-
-	while(ptr != 0){
-		/*
-		 * Check for validity.
-		 * Can't be in standard header and must be double
-		 * word aligned.
-		 */
-		if(ptr < 0x40 || (ptr & ~0xFC))
-			return -1;
-		if(pcicfgr8(p, ptr) == 0x01){
-			p->pmrb = ptr;
-			return ptr;
-		}
-
-		ptr = pcicfgr8(p, ptr+1);
-	}
-
-	return -1;
+        if(p->pmrb != 0)
+                return p->pmrb;
+        return p->pmrb = pcicap(p, PciCapPMG);
 }
 
 int