[PATCH 3/3] PCI: vmd: Restore domain info during resets/unloads

[Jon Derrick <jonathan.derrick@intel.com>]

Persistent domain info written by VMD BIOS needs to be restored by the
driver during module unloads or resets. This adds a remove or reset
action to restore the parsed domain info to all enabled VMD Root Ports.

Signed-off-by: Jon Derrick <jonathan.derrick@intel.com>
Reviewed-by: Andy Shevchenko <andriy.shevchenko@linux.intel.com>
---
 drivers/pci/controller/vmd.c | 32 ++++++++++++++++++++++++++++++++
 1 file changed, 32 insertions(+)

diff --git a/drivers/pci/controller/vmd.c b/drivers/pci/controller/vmd.c
index dbe1bff..853aa93 100644
--- a/drivers/pci/controller/vmd.c
+++ b/drivers/pci/controller/vmd.c
@@ -562,6 +562,33 @@ static int vmd_pci_write(struct pci_bus *bus, unsigned int devfn, int reg,
 				 PCI_VENDOR_ID, 4, &temp) ||		\
 		    temp == 0xffffffff) {} else
 
+static void vmd_restore_domain(void *data)
+{
+	struct vmd_dev *vmd = data;
+	int root_port;
+	u32 temp, iobase;
+
+	/*
+	 * It shouldn't be possible for the Root Port layout to change
+	 * dynamically (outside of BIOS), however there is no harm in writing
+	 * the persistent data back to all enabled Root Ports. PCI resource
+	 * assignment will discard any modifications on the next VMD domain bus
+	 * scan following VMD reset/probe.
+	 */
+	for_each_vmd_root_port(vmd, root_port, temp) {
+		if (vmd_cfg_read(vmd, 0, PCI_DEVFN(root_port, 0),
+				 PCI_IO_BASE, 2, &iobase))
+			return;
+
+		iobase &= ~((0xf << 4) | (0x3 << 14));
+		iobase |= vmd->socket_nr << 4 | vmd->instance_nr << 14;
+
+		if (vmd_cfg_write(vmd, 0, PCI_DEVFN(root_port, 0),
+				  PCI_IO_BASE, 2, iobase))
+			return;
+	}
+}
+
 static int vmd_parse_domain(struct vmd_dev *vmd)
 {
 	int root_port, ret;
@@ -579,6 +606,11 @@ static int vmd_parse_domain(struct vmd_dev *vmd)
 		vmd->socket_nr = (iobase >> 4) & 0xf;
 		vmd->instance_nr = (iobase >> 14) & 0x3;
 
+		ret = devm_add_action_or_reset(&vmd->dev->dev,
+					       vmd_restore_domain, vmd);
+		if (ret)
+			return ret;
+
 		/* First available will be used */
 		break;
 	}
-- 
1.8.3.1