1 files changed, 16 insertions, 106 deletions

diff --git a/drivers/pci/msi.c b/drivers/pci/msi.c
index 27a0574..75d1c0d 100644
--- a/drivers/pci/msi.c
+++ b/drivers/pci/msi.c
@@ -933,7 +933,6 @@ int pci_msi_supported(struct pci_dev * dev)
 int pci_enable_msi(struct pci_dev* dev)
 {
 	int pos, temp, status;
-	u16 control;
 
 	if (pci_msi_supported(dev) < 0)
 		return -EINVAL;
@@ -948,27 +947,8 @@ int pci_enable_msi(struct pci_dev* dev)
 	if (!pos)
 		return -EINVAL;
 
-	if (!msi_lookup_vector(dev, PCI_CAP_ID_MSI)) {
-		/* Lookup Sucess */
-		unsigned long flags;
+	WARN_ON(!msi_lookup_vector(dev, PCI_CAP_ID_MSI));
 
-		pci_read_config_word(dev, msi_control_reg(pos), &control);
-		if (control & PCI_MSI_FLAGS_ENABLE)
-			return 0;	/* Already in MSI mode */
-		spin_lock_irqsave(&msi_lock, flags);
-		if (!vector_irq[dev->irq]) {
-			msi_desc[dev->irq]->msi_attrib.state = 0;
-			vector_irq[dev->irq] = -1;
-			nr_released_vectors--;
-			spin_unlock_irqrestore(&msi_lock, flags);
-			status = msi_register_init(dev, msi_desc[dev->irq]);
-			if (status == 0)
-				enable_msi_mode(dev, pos, PCI_CAP_ID_MSI);
-			return status;
-		}
-		spin_unlock_irqrestore(&msi_lock, flags);
-		dev->irq = temp;
-	}
 	/* Check whether driver already requested for MSI-X vectors */
 	pos = pci_find_capability(dev, PCI_CAP_ID_MSIX);
 	if (pos > 0 && !msi_lookup_vector(dev, PCI_CAP_ID_MSIX)) {
@@ -1010,6 +990,8 @@ void pci_disable_msi(struct pci_dev* dev)
 	if (!(control & PCI_MSI_FLAGS_ENABLE))
 		return;
 
+	disable_msi_mode(dev, pos, PCI_CAP_ID_MSI);
+
 	spin_lock_irqsave(&msi_lock, flags);
 	entry = msi_desc[dev->irq];
 	if (!entry || !entry->dev || entry->msi_attrib.type != PCI_CAP_ID_MSI) {
@@ -1023,14 +1005,12 @@ void pci_disable_msi(struct pci_dev* dev)
 		       pci_name(dev), dev->irq);
 		BUG_ON(entry->msi_attrib.state > 0);
 	} else {
-		vector_irq[dev->irq] = 0; /* free it */
-		nr_released_vectors++;
 		default_vector = entry->msi_attrib.default_vector;
 		spin_unlock_irqrestore(&msi_lock, flags);
+		msi_free_vector(dev, dev->irq, 0);
+
 		/* Restore dev->irq to its default pin-assertion vector */
 		dev->irq = default_vector;
-		disable_msi_mode(dev, pci_find_capability(dev, PCI_CAP_ID_MSI),
-					PCI_CAP_ID_MSI);
 	}
 }
 
@@ -1078,57 +1058,6 @@ static int msi_free_vector(struct pci_dev* dev, int vector, int reassign)
 	return 0;
 }
 
-static int reroute_msix_table(int head, struct msix_entry *entries, int *nvec)
-{
-	int vector = head, tail = 0;
-	int i, j = 0, nr_entries = 0;
-	void __iomem *base;
-	unsigned long flags;
-
-	spin_lock_irqsave(&msi_lock, flags);
-	while (head != tail) {
-		nr_entries++;
-		tail = msi_desc[vector]->link.tail;
-		if (entries[0].entry == msi_desc[vector]->msi_attrib.entry_nr)
-			j = vector;
-		vector = tail;
-	}
-	if (*nvec > nr_entries) {
-		spin_unlock_irqrestore(&msi_lock, flags);
-		*nvec = nr_entries;
-		return -EINVAL;
-	}
-	vector = ((j > 0) ? j : head);
-	for (i = 0; i < *nvec; i++) {
-		j = msi_desc[vector]->msi_attrib.entry_nr;
-		msi_desc[vector]->msi_attrib.state = 0;	/* Mark it not active */
-		vector_irq[vector] = -1;		/* Mark it busy */
-		nr_released_vectors--;
-		entries[i].vector = vector;
-		if (j != (entries + i)->entry) {
-			base = msi_desc[vector]->mask_base;
-			msi_desc[vector]->msi_attrib.entry_nr =
-				(entries + i)->entry;
-			writel( readl(base + j * PCI_MSIX_ENTRY_SIZE +
-				PCI_MSIX_ENTRY_LOWER_ADDR_OFFSET), base +
-				(entries + i)->entry * PCI_MSIX_ENTRY_SIZE +
-				PCI_MSIX_ENTRY_LOWER_ADDR_OFFSET);
-			writel(	readl(base + j * PCI_MSIX_ENTRY_SIZE +
-				PCI_MSIX_ENTRY_UPPER_ADDR_OFFSET), base +
-				(entries + i)->entry * PCI_MSIX_ENTRY_SIZE +
-				PCI_MSIX_ENTRY_UPPER_ADDR_OFFSET);
-			writel( (readl(base + j * PCI_MSIX_ENTRY_SIZE +
-				PCI_MSIX_ENTRY_DATA_OFFSET) & 0xff00) | vector,
-				base + (entries+i)->entry*PCI_MSIX_ENTRY_SIZE +
-				PCI_MSIX_ENTRY_DATA_OFFSET);
-		}
-		vector = msi_desc[vector]->link.tail;
-	}
-	spin_unlock_irqrestore(&msi_lock, flags);
-
-	return 0;
-}
-
 /**
  * pci_enable_msix - configure device's MSI-X capability structure
  * @dev: pointer to the pci_dev data structure of MSI-X device function
@@ -1163,9 +1092,6 @@ int pci_enable_msix(struct pci_dev* dev, struct msix_entry *entries, int nvec)
  		return -EINVAL;
 
 	pci_read_config_word(dev, msi_control_reg(pos), &control);
-	if (control & PCI_MSIX_FLAGS_ENABLE)
-		return -EINVAL;			/* Already in MSI-X mode */
-
 	nr_entries = multi_msix_capable(control);
 	if (nvec > nr_entries)
 		return -EINVAL;
@@ -1180,19 +1106,8 @@ int pci_enable_msix(struct pci_dev* dev, struct msix_entry *entries, int nvec)
 		}
 	}
 	temp = dev->irq;
-	if (!msi_lookup_vector(dev, PCI_CAP_ID_MSIX)) {
-		/* Lookup Sucess */
-		nr_entries = nvec;
-		/* Reroute MSI-X table */
-		if (reroute_msix_table(dev->irq, entries, &nr_entries)) {
-			/* #requested > #previous-assigned */
-			dev->irq = temp;
-			return nr_entries;
-		}
-		dev->irq = temp;
-		enable_msi_mode(dev, pos, PCI_CAP_ID_MSIX);
-		return 0;
-	}
+	WARN_ON(!msi_lookup_vector(dev, PCI_CAP_ID_MSIX));
+
 	/* Check whether driver already requested for MSI vector */
    	if (pci_find_capability(dev, PCI_CAP_ID_MSI) > 0 &&
 		!msi_lookup_vector(dev, PCI_CAP_ID_MSI)) {
@@ -1251,37 +1166,32 @@ void pci_disable_msix(struct pci_dev* dev)
 	if (!(control & PCI_MSIX_FLAGS_ENABLE))
 		return;
 
+	disable_msi_mode(dev, pos, PCI_CAP_ID_MSIX);
+
 	temp = dev->irq;
 	if (!msi_lookup_vector(dev, PCI_CAP_ID_MSIX)) {
 		int state, vector, head, tail = 0, warning = 0;
 		unsigned long flags;
 
 		vector = head = dev->irq;
-		spin_lock_irqsave(&msi_lock, flags);
+		dev->irq = temp;			/* Restore pin IRQ */
 		while (head != tail) {
+			spin_lock_irqsave(&msi_lock, flags);
 			state = msi_desc[vector]->msi_attrib.state;
+			tail = msi_desc[vector]->link.tail;
+			spin_unlock_irqrestore(&msi_lock, flags);
 			if (state)
 				warning = 1;
-			else {
-				vector_irq[vector] = 0; /* free it */
-				nr_released_vectors++;
-			}
-			tail = msi_desc[vector]->link.tail;
+			else if (vector != head)	/* Release MSI-X vector */
+				msi_free_vector(dev, vector, 0);
 			vector = tail;
 		}
-		spin_unlock_irqrestore(&msi_lock, flags);
+		msi_free_vector(dev, vector, 0);
 		if (warning) {
-			dev->irq = temp;
 			printk(KERN_WARNING "PCI: %s: pci_disable_msix() called without "
 			       "free_irq() on all MSI-X vectors\n",
 			       pci_name(dev));
 			BUG_ON(warning > 0);
-		} else {
-			dev->irq = temp;
-			disable_msi_mode(dev,
-				pci_find_capability(dev, PCI_CAP_ID_MSIX),
-				PCI_CAP_ID_MSIX);
-
 		}
 	}
 }