1 files changed, 86 insertions, 199 deletions

diff --git a/drivers/pci/msi.c b/drivers/pci/msi.c
index 68555c11f55..435c1958a7b 100644
--- a/drivers/pci/msi.c
+++ b/drivers/pci/msi.c
@@ -38,6 +38,36 @@ static int msi_cache_init(void)
 	return 0;
 }
 
+static void msi_set_enable(struct pci_dev *dev, int enable)
+{
+	int pos;
+	u16 control;
+
+	pos = pci_find_capability(dev, PCI_CAP_ID_MSI);
+	if (pos) {
+		pci_read_config_word(dev, pos + PCI_MSI_FLAGS, &control);
+		control &= ~PCI_MSI_FLAGS_ENABLE;
+		if (enable)
+			control |= PCI_MSI_FLAGS_ENABLE;
+		pci_write_config_word(dev, pos + PCI_MSI_FLAGS, control);
+	}
+}
+
+static void msix_set_enable(struct pci_dev *dev, int enable)
+{
+	int pos;
+	u16 control;
+
+	pos = pci_find_capability(dev, PCI_CAP_ID_MSIX);
+	if (pos) {
+		pci_read_config_word(dev, pos + PCI_MSIX_FLAGS, &control);
+		control &= ~PCI_MSIX_FLAGS_ENABLE;
+		if (enable)
+			control |= PCI_MSIX_FLAGS_ENABLE;
+		pci_write_config_word(dev, pos + PCI_MSIX_FLAGS, control);
+	}
+}
+
 static void msi_set_mask_bit(unsigned int irq, int flag)
 {
 	struct msi_desc *entry;
@@ -55,6 +85,8 @@ static void msi_set_mask_bit(unsigned int irq, int flag)
 			mask_bits &= ~(1);
 			mask_bits |= flag;
 			pci_write_config_dword(entry->dev, pos, mask_bits);
+		} else {
+			msi_set_enable(entry->dev, !flag);
 		}
 		break;
 	case PCI_CAP_ID_MSIX:
@@ -62,12 +94,14 @@ static void msi_set_mask_bit(unsigned int irq, int flag)
 		int offset = entry->msi_attrib.entry_nr * PCI_MSIX_ENTRY_SIZE +
 			PCI_MSIX_ENTRY_VECTOR_CTRL_OFFSET;
 		writel(flag, entry->mask_base + offset);
+		readl(entry->mask_base + offset);
 		break;
 	}
 	default:
 		BUG();
 		break;
 	}
+	entry->msi_attrib.masked = !!flag;
 }
 
 void read_msi_msg(unsigned int irq, struct msi_msg *msg)
@@ -147,6 +181,7 @@ void write_msi_msg(unsigned int irq, struct msi_msg *msg)
 	default:
 		BUG();
 	}
+	entry->msg = *msg;
 }
 
 void mask_msi_irq(unsigned int irq)
@@ -192,201 +227,61 @@ static struct msi_desc* alloc_msi_entry(void)
 	return entry;
 }
 
-static void enable_msi_mode(struct pci_dev *dev, int pos, int type)
-{
-	u16 control;
-
-	pci_read_config_word(dev, msi_control_reg(pos), &control);
-	if (type == PCI_CAP_ID_MSI) {
-		/* Set enabled bits to single MSI & enable MSI_enable bit */
-		msi_enable(control, 1);
-		pci_write_config_word(dev, msi_control_reg(pos), control);
-		dev->msi_enabled = 1;
-	} else {
-		msix_enable(control);
-		pci_write_config_word(dev, msi_control_reg(pos), control);
-		dev->msix_enabled = 1;
-	}
-
-	pci_intx(dev, 0);  /* disable intx */
-}
-
-void disable_msi_mode(struct pci_dev *dev, int pos, int type)
-{
-	u16 control;
-
-	pci_read_config_word(dev, msi_control_reg(pos), &control);
-	if (type == PCI_CAP_ID_MSI) {
-		/* Set enabled bits to single MSI & enable MSI_enable bit */
-		msi_disable(control);
-		pci_write_config_word(dev, msi_control_reg(pos), control);
-		dev->msi_enabled = 0;
-	} else {
-		msix_disable(control);
-		pci_write_config_word(dev, msi_control_reg(pos), control);
-		dev->msix_enabled = 0;
-	}
-
-	pci_intx(dev, 1);  /* enable intx */
-}
-
 #ifdef CONFIG_PM
-static int __pci_save_msi_state(struct pci_dev *dev)
-{
-	int pos, i = 0;
-	u16 control;
-	struct pci_cap_saved_state *save_state;
-	u32 *cap;
-
-	pos = pci_find_capability(dev, PCI_CAP_ID_MSI);
-	if (pos <= 0 || dev->no_msi)
-		return 0;
-
-	pci_read_config_word(dev, msi_control_reg(pos), &control);
-	if (!(control & PCI_MSI_FLAGS_ENABLE))
-		return 0;
-
-	save_state = kzalloc(sizeof(struct pci_cap_saved_state) + sizeof(u32) * 5,
-		GFP_KERNEL);
-	if (!save_state) {
-		printk(KERN_ERR "Out of memory in pci_save_msi_state\n");
-		return -ENOMEM;
-	}
-	cap = &save_state->data[0];
-
-	pci_read_config_dword(dev, pos, &cap[i++]);
-	control = cap[0] >> 16;
-	pci_read_config_dword(dev, pos + PCI_MSI_ADDRESS_LO, &cap[i++]);
-	if (control & PCI_MSI_FLAGS_64BIT) {
-		pci_read_config_dword(dev, pos + PCI_MSI_ADDRESS_HI, &cap[i++]);
-		pci_read_config_dword(dev, pos + PCI_MSI_DATA_64, &cap[i++]);
-	} else
-		pci_read_config_dword(dev, pos + PCI_MSI_DATA_32, &cap[i++]);
-	if (control & PCI_MSI_FLAGS_MASKBIT)
-		pci_read_config_dword(dev, pos + PCI_MSI_MASK_BIT, &cap[i++]);
-	save_state->cap_nr = PCI_CAP_ID_MSI;
-	pci_add_saved_cap(dev, save_state);
-	return 0;
-}
-
 static void __pci_restore_msi_state(struct pci_dev *dev)
 {
-	int i = 0, pos;
-	u16 control;
-	struct pci_cap_saved_state *save_state;
-	u32 *cap;
-
-	save_state = pci_find_saved_cap(dev, PCI_CAP_ID_MSI);
-	pos = pci_find_capability(dev, PCI_CAP_ID_MSI);
-	if (!save_state || pos <= 0)
-		return;
-	cap = &save_state->data[0];
-
-	control = cap[i++] >> 16;
-	pci_write_config_dword(dev, pos + PCI_MSI_ADDRESS_LO, cap[i++]);
-	if (control & PCI_MSI_FLAGS_64BIT) {
-		pci_write_config_dword(dev, pos + PCI_MSI_ADDRESS_HI, cap[i++]);
-		pci_write_config_dword(dev, pos + PCI_MSI_DATA_64, cap[i++]);
-	} else
-		pci_write_config_dword(dev, pos + PCI_MSI_DATA_32, cap[i++]);
-	if (control & PCI_MSI_FLAGS_MASKBIT)
-		pci_write_config_dword(dev, pos + PCI_MSI_MASK_BIT, cap[i++]);
-	pci_write_config_word(dev, pos + PCI_MSI_FLAGS, control);
-	enable_msi_mode(dev, pos, PCI_CAP_ID_MSI);
-	pci_remove_saved_cap(save_state);
-	kfree(save_state);
-}
-
-static int __pci_save_msix_state(struct pci_dev *dev)
-{
 	int pos;
-	int irq, head, tail = 0;
 	u16 control;
-	struct pci_cap_saved_state *save_state;
-
-	if (!dev->msix_enabled)
-		return 0;
-
-	pos = pci_find_capability(dev, PCI_CAP_ID_MSIX);
-	if (pos <= 0 || dev->no_msi)
-		return 0;
-
-	/* save the capability */
-	pci_read_config_word(dev, msi_control_reg(pos), &control);
-	if (!(control & PCI_MSIX_FLAGS_ENABLE))
-		return 0;
-	save_state = kzalloc(sizeof(struct pci_cap_saved_state) + sizeof(u16),
-		GFP_KERNEL);
-	if (!save_state) {
-		printk(KERN_ERR "Out of memory in pci_save_msix_state\n");
-		return -ENOMEM;
-	}
-	*((u16 *)&save_state->data[0]) = control;
-
-	/* save the table */
-	irq = head = dev->first_msi_irq;
-	while (head != tail) {
-		struct msi_desc *entry;
-
-		entry = get_irq_msi(irq);
-		read_msi_msg(irq, &entry->msg_save);
-
-		tail = entry->link.tail;
-		irq = tail;
-	}
-
-	save_state->cap_nr = PCI_CAP_ID_MSIX;
-	pci_add_saved_cap(dev, save_state);
-	return 0;
-}
+	struct msi_desc *entry;
 
-int pci_save_msi_state(struct pci_dev *dev)
-{
-	int rc;
+	if (!dev->msi_enabled)
+		return;
 
-	rc = __pci_save_msi_state(dev);
-	if (rc)
-		return rc;
+	entry = get_irq_msi(dev->irq);
+	pos = entry->msi_attrib.pos;
 
-	rc = __pci_save_msix_state(dev);
+	pci_intx(dev, 0);		/* disable intx */
+	msi_set_enable(dev, 0);
+	write_msi_msg(dev->irq, &entry->msg);
+	if (entry->msi_attrib.maskbit)
+		msi_set_mask_bit(dev->irq, entry->msi_attrib.masked);
 
-	return rc;
+	pci_read_config_word(dev, pos + PCI_MSI_FLAGS, &control);
+	control &= ~(PCI_MSI_FLAGS_QSIZE | PCI_MSI_FLAGS_ENABLE);
+	if (entry->msi_attrib.maskbit || !entry->msi_attrib.masked)
+		control |= PCI_MSI_FLAGS_ENABLE;
+	pci_write_config_word(dev, pos + PCI_MSI_FLAGS, control);
 }
 
 static void __pci_restore_msix_state(struct pci_dev *dev)
 {
-	u16 save;
 	int pos;
 	int irq, head, tail = 0;
 	struct msi_desc *entry;
-	struct pci_cap_saved_state *save_state;
+	u16 control;
 
 	if (!dev->msix_enabled)
 		return;
 
-	save_state = pci_find_saved_cap(dev, PCI_CAP_ID_MSIX);
-	if (!save_state)
-		return;
-	save = *((u16 *)&save_state->data[0]);
-	pci_remove_saved_cap(save_state);
-	kfree(save_state);
-
-	pos = pci_find_capability(dev, PCI_CAP_ID_MSIX);
-	if (pos <= 0)
-		return;
-
 	/* route the table */
+	pci_intx(dev, 0);		/* disable intx */
+	msix_set_enable(dev, 0);
 	irq = head = dev->first_msi_irq;
+	entry = get_irq_msi(irq);
+	pos = entry->msi_attrib.pos;
 	while (head != tail) {
 		entry = get_irq_msi(irq);
-		write_msi_msg(irq, &entry->msg_save);
+		write_msi_msg(irq, &entry->msg);
+		msi_set_mask_bit(irq, entry->msi_attrib.masked);
 
 		tail = entry->link.tail;
 		irq = tail;
 	}
 
-	pci_write_config_word(dev, msi_control_reg(pos), save);
-	enable_msi_mode(dev, pos, PCI_CAP_ID_MSIX);
+	pci_read_config_word(dev, pos + PCI_MSIX_FLAGS, &control);
+	control &= ~PCI_MSIX_FLAGS_MASKALL;
+	control |= PCI_MSIX_FLAGS_ENABLE;
+	pci_write_config_word(dev, pos + PCI_MSIX_FLAGS, control);
 }
 
 void pci_restore_msi_state(struct pci_dev *dev)
@@ -411,6 +306,8 @@ static int msi_capability_init(struct pci_dev *dev)
 	int pos, irq;
 	u16 control;
 
+	msi_set_enable(dev, 0);	/* Ensure msi is disabled as I set it up */
+
    	pos = pci_find_capability(dev, PCI_CAP_ID_MSI);
 	pci_read_config_word(dev, msi_control_reg(pos), &control);
 	/* MSI Entry Initialization */
@@ -422,6 +319,7 @@ static int msi_capability_init(struct pci_dev *dev)
 	entry->msi_attrib.is_64 = is_64bit_address(control);
 	entry->msi_attrib.entry_nr = 0;
 	entry->msi_attrib.maskbit = is_mask_bit_support(control);
+	entry->msi_attrib.masked = 1;
 	entry->msi_attrib.default_irq = dev->irq;	/* Save IOAPIC IRQ */
 	entry->msi_attrib.pos = pos;
 	if (is_mask_bit_support(control)) {
@@ -454,7 +352,9 @@ static int msi_capability_init(struct pci_dev *dev)
 	set_irq_msi(irq, entry);
 
 	/* Set MSI enabled bits	 */
-	enable_msi_mode(dev, pos, PCI_CAP_ID_MSI);
+	pci_intx(dev, 0);		/* disable intx */
+	msi_set_enable(dev, 1);
+	dev->msi_enabled = 1;
 
 	dev->irq = irq;
 	return 0;
@@ -481,6 +381,8 @@ static int msix_capability_init(struct pci_dev *dev,
 	u8 bir;
 	void __iomem *base;
 
+	msix_set_enable(dev, 0);/* Ensure msix is disabled as I set it up */
+
    	pos = pci_find_capability(dev, PCI_CAP_ID_MSIX);
 	/* Request & Map MSI-X table region */
  	pci_read_config_word(dev, msi_control_reg(pos), &control);
@@ -505,6 +407,7 @@ static int msix_capability_init(struct pci_dev *dev,
 		entry->msi_attrib.is_64 = 1;
 		entry->msi_attrib.entry_nr = j;
 		entry->msi_attrib.maskbit = 1;
+		entry->msi_attrib.masked = 1;
 		entry->msi_attrib.default_irq = dev->irq;
 		entry->msi_attrib.pos = pos;
 		entry->dev = dev;
@@ -549,7 +452,9 @@ static int msix_capability_init(struct pci_dev *dev,
 	}
 	dev->first_msi_irq = entries[0].vector;
 	/* Set MSI-X enabled bits */
-	enable_msi_mode(dev, pos, PCI_CAP_ID_MSIX);
+	pci_intx(dev, 0);		/* disable intx */
+	msix_set_enable(dev, 1);
+	dev->msix_enabled = 1;
 
 	return 0;
 }
@@ -611,12 +516,11 @@ int pci_enable_msi(struct pci_dev* dev)
 	WARN_ON(!!dev->msi_enabled);
 
 	/* Check whether driver already requested for MSI-X irqs */
-	pos = pci_find_capability(dev, PCI_CAP_ID_MSIX);
-	if (pos > 0 && dev->msix_enabled) {
-			printk(KERN_INFO "PCI: %s: Can't enable MSI.  "
-			       "Device already has MSI-X enabled\n",
-			       pci_name(dev));
-			return -EINVAL;
+	if (dev->msix_enabled) {
+		printk(KERN_INFO "PCI: %s: Can't enable MSI.  "
+			"Device already has MSI-X enabled\n",
+			pci_name(dev));
+		return -EINVAL;
 	}
 	status = msi_capability_init(dev);
 	return status;
@@ -625,8 +529,7 @@ int pci_enable_msi(struct pci_dev* dev)
 void pci_disable_msi(struct pci_dev* dev)
 {
 	struct msi_desc *entry;
-	int pos, default_irq;
-	u16 control;
+	int default_irq;
 
 	if (!pci_msi_enable)
 		return;
@@ -636,16 +539,9 @@ void pci_disable_msi(struct pci_dev* dev)
 	if (!dev->msi_enabled)
 		return;
 
-	pos = pci_find_capability(dev, PCI_CAP_ID_MSI);
-	if (!pos)
-		return;
-
-	pci_read_config_word(dev, msi_control_reg(pos), &control);
-	if (!(control & PCI_MSI_FLAGS_ENABLE))
-		return;
-
-
-	disable_msi_mode(dev, pos, PCI_CAP_ID_MSI);
+	msi_set_enable(dev, 0);
+	pci_intx(dev, 1);		/* enable intx */
+	dev->msi_enabled = 0;
 
 	entry = get_irq_msi(dev->first_msi_irq);
 	if (!entry || !entry->dev || entry->msi_attrib.type != PCI_CAP_ID_MSI) {
@@ -746,8 +642,7 @@ int pci_enable_msix(struct pci_dev* dev, struct msix_entry *entries, int nvec)
 	WARN_ON(!!dev->msix_enabled);
 
 	/* Check whether driver already requested for MSI irq */
-   	if (pci_find_capability(dev, PCI_CAP_ID_MSI) > 0 &&
-		dev->msi_enabled) {
+   	if (dev->msi_enabled) {
 		printk(KERN_INFO "PCI: %s: Can't enable MSI-X.  "
 		       "Device already has an MSI irq assigned\n",
 		       pci_name(dev));
@@ -760,8 +655,6 @@ int pci_enable_msix(struct pci_dev* dev, struct msix_entry *entries, int nvec)
 void pci_disable_msix(struct pci_dev* dev)
 {
 	int irq, head, tail = 0, warning = 0;
-	int pos;
-	u16 control;
 
 	if (!pci_msi_enable)
 		return;
@@ -771,15 +664,9 @@ void pci_disable_msix(struct pci_dev* dev)
 	if (!dev->msix_enabled)
 		return;
 
-	pos = pci_find_capability(dev, PCI_CAP_ID_MSIX);
-	if (!pos)
-		return;
-
-	pci_read_config_word(dev, msi_control_reg(pos), &control);
-	if (!(control & PCI_MSIX_FLAGS_ENABLE))
-		return;
-
-	disable_msi_mode(dev, pos, PCI_CAP_ID_MSIX);
+	msix_set_enable(dev, 0);
+	pci_intx(dev, 1);		/* enable intx */
+	dev->msix_enabled = 0;
 
 	irq = head = dev->first_msi_irq;
 	while (head != tail) {