Revert PCI config space writes on shutdown

This means all chipset enables etc. will be undone on shutdown.
Reversible PCI config space writes now use rpci_write_*().
PCI config space writes which are one-shot (e.g. communication via
config space) should continue to use the permanent pci_write_*
variants.

Extend the number of available register_shutdown slots to 32.

Corresponding to flashrom svn r1232.

Signed-off-by: Carl-Daniel Hailfinger <c-d.hailfinger.devel.2006@gmx.net>
Acked-by: Michael Karcher <flashrom@mkarcher.dialup.fu-berlin.de>
diff --git a/atahpt.c b/atahpt.c
index 2552697..eac056e 100644
--- a/atahpt.c
+++ b/atahpt.c
@@ -50,7 +50,7 @@
 	/* Enable flash access. */
 	reg32 = pci_read_long(pcidev_dev, REG_FLASH_ACCESS);
 	reg32 |= (1 << 24);
-	pci_write_long(pcidev_dev, REG_FLASH_ACCESS, reg32);
+	rpci_write_long(pcidev_dev, REG_FLASH_ACCESS, reg32);
 
 	buses_supported = CHIP_BUSTYPE_PARALLEL;
 
@@ -59,13 +59,7 @@
 
 int atahpt_shutdown(void)
 {
-	uint32_t reg32;
-
-	/* Disable flash access again. */
-	reg32 = pci_read_long(pcidev_dev, REG_FLASH_ACCESS);
-	reg32 &= ~(1 << 24);
-	pci_write_long(pcidev_dev, REG_FLASH_ACCESS, reg32);
-
+	/* Flash access is disabled automatically by PCI restore. */
 	pci_cleanup(pacc);
 	release_io_perms();
 	return 0;
diff --git a/chipset_enable.c b/chipset_enable.c
index 96c240c..ec15996 100644
--- a/chipset_enable.c
+++ b/chipset_enable.c
@@ -47,7 +47,7 @@
 	 */
 	tmp = pci_read_byte(dev, 0x47);
 	tmp |= 0x46;
-	pci_write_byte(dev, 0x47, tmp);
+	rpci_write_byte(dev, 0x47, tmp);
 
 	return 0;
 }
@@ -58,7 +58,7 @@
 
 	tmp = pci_read_byte(dev, 0xd0);
 	tmp |= 0xf8;
-	pci_write_byte(dev, 0xd0, tmp);
+	rpci_write_byte(dev, 0xd0, tmp);
 
 	return 0;
 }
@@ -72,7 +72,7 @@
 	new = pci_read_byte(dev, 0x40);
 	new &= (~0x04); /* No idea why we clear bit 2. */
 	new |= 0xb; /* 0x3 for some chipsets, bit 7 seems to be don't care. */
-	pci_write_byte(dev, 0x40, new);
+	rpci_write_byte(dev, 0x40, new);
 	newer = pci_read_byte(dev, 0x40);
 	if (newer != new) {
 		msg_pinfo("tried to set register 0x%x to 0x%x on %s failed (WARNING ONLY)\n", 0x40, new, name);
@@ -160,7 +160,7 @@
 	new = pci_read_byte(sbdev, 0x45);
 	new &= (~0x20);
 	new |= 0x4;
-	pci_write_byte(sbdev, 0x45, new);
+	rpci_write_byte(sbdev, 0x45, new);
 	newer = pci_read_byte(sbdev, 0x45);
 	if (newer != new) {
 		msg_pinfo("tried to set register 0x%x to 0x%x on %s failed (WARNING ONLY)\n", 0x45, new, name);
@@ -186,7 +186,7 @@
 	new = pci_read_byte(sbdev, 0x45);
 	new &= (~0x80);
 	new |= 0x40;
-	pci_write_byte(sbdev, 0x45, new);
+	rpci_write_byte(sbdev, 0x45, new);
 	newer = pci_read_byte(sbdev, 0x45);
 	if (newer != new) {
 		msg_pinfo("tried to set register 0x%x to 0x%x on %s failed (WARNING ONLY)\n", 0x45, new, name);
@@ -233,7 +233,7 @@
 	if (new == old)
 		return 0;
 
-	pci_write_word(dev, xbcs, new);
+	rpci_write_word(dev, xbcs, new);
 
 	if (pci_read_word(dev, xbcs) != new) {
 		msg_pinfo("tried to set 0x%x to 0x%x on %s failed (WARNING ONLY)\n", xbcs, new, name);
@@ -269,7 +269,7 @@
 	if (new == old)
 		return 0;
 
-	pci_write_byte(dev, bios_cntl, new);
+	rpci_write_byte(dev, bios_cntl, new);
 
 	if (pci_read_byte(dev, bios_cntl) != new) {
 		msg_pinfo("tried to set 0x%x to 0x%x on %s failed (WARNING ONLY)\n", bios_cntl, new, name);
@@ -306,8 +306,8 @@
 
 		/* FIXME: Need to undo this on shutdown. */
 		msg_pinfo("\nSetting IDSEL=0x%x for top 16 MB", fwh_conf);
-		pci_write_long(dev, 0xd0, fwh_conf);
-		pci_write_word(dev, 0xd4, fwh_conf);
+		rpci_write_long(dev, 0xd0, fwh_conf);
+		rpci_write_word(dev, 0xd4, fwh_conf);
 		/* FIXME: Decode settings are not changed. */
 	} else if (idsel) {
 		msg_perr("Error: idsel= specified, but no number given.\n");
@@ -403,7 +403,7 @@
        new = old & ~1;
 
        if (new != old)
-               pci_write_byte(dev, 0xd9, new);
+               rpci_write_byte(dev, 0xd9, new);
 
 	buses_supported = CHIP_BUSTYPE_FWH;
        return 0;
@@ -498,17 +498,6 @@
 	return enable_flash_ich_dc_spi(dev, name, 10);
 }
 
-static void via_do_byte_merge(void * arg)
-{
-	struct pci_dev * dev = arg;
-	uint8_t val;
-
-	msg_pdbg("Re-enabling byte merging\n");
-	val = pci_read_byte(dev, 0x71);
-	val |= 0x40;
-	pci_write_byte(dev, 0x71, val);
-}
-
 static int via_no_byte_merge(struct pci_dev *dev, const char *name)
 {
 	uint8_t val;
@@ -518,8 +507,7 @@
 	{
 		msg_pdbg("Disabling byte merging\n");
 		val &= ~0x40;
-		pci_write_byte(dev, 0x71, val);
-		register_shutdown(via_do_byte_merge, dev);
+		rpci_write_byte(dev, 0x71, val);
 	}
 	return NOT_DONE_YET;	/* need to find south bridge, too */
 }
@@ -529,12 +517,12 @@
 	uint8_t val;
 
 	/* enable ROM decode range (1MB) FFC00000 - FFFFFFFF */
-	pci_write_byte(dev, 0x41, 0x7f);
+	rpci_write_byte(dev, 0x41, 0x7f);
 
 	/* ROM write enable */
 	val = pci_read_byte(dev, 0x40);
 	val |= 0x10;
-	pci_write_byte(dev, 0x40, val);
+	rpci_write_byte(dev, 0x40, val);
 
 	if (pci_read_byte(dev, 0x40) != val) {
 		msg_pinfo("\nWARNING: Failed to enable flash write on \"%s\"\n",
@@ -546,7 +534,7 @@
 	    /* All memory cycles, not just ROM ones, go to LPC. */
 	    val = pci_read_byte(dev, 0x59);
 	    val &= ~0x80;
-	    pci_write_byte(dev, 0x59, val);
+	    rpci_write_byte(dev, 0x59, val);
 	}
 
 	return 0;
@@ -580,12 +568,12 @@
 	reg8 |= LOWER_ROM_ADDRESS_RANGE;
 	reg8 |= UPPER_ROM_ADDRESS_RANGE;
 	reg8 |= ROM_WRITE_ENABLE;
-	pci_write_byte(dev, ROM_AT_LOGIC_CONTROL_REG, reg8);
+	rpci_write_byte(dev, ROM_AT_LOGIC_CONTROL_REG, reg8);
 
 	/* Set positive decode on ROM. */
 	reg8 = pci_read_byte(dev, DECODE_CONTROL_REG2);
 	reg8 |= BIOS_ROM_POSITIVE_DECODE;
-	pci_write_byte(dev, DECODE_CONTROL_REG2, reg8);
+	rpci_write_byte(dev, DECODE_CONTROL_REG2, reg8);
 
 	reg8 = pci_read_byte(dev, CS5530_RESET_CONTROL_REG);
 	if (reg8 & CS5530_ISA_MASTER) {
@@ -649,7 +637,7 @@
 {
 	uint8_t new;
 
-	pci_write_byte(dev, 0x52, 0xee);
+	rpci_write_byte(dev, 0x52, 0xee);
 
 	new = pci_read_byte(dev, 0x52);
 
@@ -670,7 +658,7 @@
 	old = pci_read_byte(dev, 0x43);
 	new = old | 0xC0;
 	if (new != old) {
-		pci_write_byte(dev, 0x43, new);
+		rpci_write_byte(dev, 0x43, new);
 		if (pci_read_byte(dev, 0x43) != new) {
 			msg_pinfo("tried to set 0x%x to 0x%x on %s failed (WARNING ONLY)\n", 0x43, new, name);
 		}
@@ -681,7 +669,7 @@
 	new = old | 0x01;
 	if (new == old)
 		return 0;
-	pci_write_byte(dev, 0x40, new);
+	rpci_write_byte(dev, 0x40, new);
 
 	if (pci_read_byte(dev, 0x40) != new) {
 		msg_pinfo("tried to set 0x%x to 0x%x on %s failed (WARNING ONLY)\n", 0x40, new, name);
@@ -709,7 +697,7 @@
 			(prot & 0xfffffc00),
 			(prot & 0xfffffc00) + ((prot & 0x3ff) << 8));
 		prot &= 0xfffffffc;
-		pci_write_byte(dev, reg, prot);
+		rpci_write_byte(dev, reg, prot);
 		prot = pci_read_long(dev, reg);
 		if (prot & 0x3)
 			msg_perr("SB600 %s%sunprotect failed from %u to %u\n",
@@ -765,11 +753,11 @@
 {
 	uint8_t tmp;
 
-	pci_write_byte(dev, 0x92, 0);
+	rpci_write_byte(dev, 0x92, 0);
 
 	tmp = pci_read_byte(dev, 0x6d);
 	tmp |= 0x01;
-	pci_write_byte(dev, 0x6d, tmp);
+	rpci_write_byte(dev, 0x6d, tmp);
 
 	return 0;
 }
@@ -781,7 +769,7 @@
 	old = pci_read_byte(dev, 0x88);
 	new = old | 0xc0;
 	if (new != old) {
-		pci_write_byte(dev, 0x88, new);
+		rpci_write_byte(dev, 0x88, new);
 		if (pci_read_byte(dev, 0x88) != new) {
 			msg_pinfo("tried to set 0x%x to 0x%x on %s failed (WARNING ONLY)\n", 0x88, new, name);
 		}
@@ -791,7 +779,7 @@
 	new = old | 0x01;
 	if (new == old)
 		return 0;
-	pci_write_byte(dev, 0x6d, new);
+	rpci_write_byte(dev, 0x6d, new);
 
 	if (pci_read_byte(dev, 0x6d) != new) {
 		msg_pinfo("tried to set 0x%x to 0x%x on %s failed (WARNING ONLY)\n", 0x6d, new, name);
@@ -835,12 +823,12 @@
 	/* Enable some SMBus stuff. */
 	tmp = pci_read_byte(smbusdev, 0x79);
 	tmp |= 0x01;
-	pci_write_byte(smbusdev, 0x79, tmp);
+	rpci_write_byte(smbusdev, 0x79, tmp);
 
 	/* Change southbridge. */
 	tmp = pci_read_byte(dev, 0x48);
 	tmp |= 0x21;
-	pci_write_byte(dev, 0x48, tmp);
+	rpci_write_byte(dev, 0x48, tmp);
 
 	/* Now become a bit silly. */
 	tmp = INB(0xc6f);
@@ -862,19 +850,19 @@
 	/* Set the 0-16 MB enable bits. */
 	val = pci_read_byte(dev, 0x88);
 	val |= 0xff;		/* 256K */
-	pci_write_byte(dev, 0x88, val);
+	rpci_write_byte(dev, 0x88, val);
 	val = pci_read_byte(dev, 0x8c);
 	val |= 0xff;		/* 1M */
-	pci_write_byte(dev, 0x8c, val);
+	rpci_write_byte(dev, 0x8c, val);
 	wordval = pci_read_word(dev, 0x90);
 	wordval |= 0x7fff;	/* 16M */
-	pci_write_word(dev, 0x90, wordval);
+	rpci_write_word(dev, 0x90, wordval);
 
 	old = pci_read_byte(dev, 0x6d);
 	new = old | 0x01;
 	if (new == old)
 		return 0;
-	pci_write_byte(dev, 0x6d, new);
+	rpci_write_byte(dev, 0x6d, new);
 
 	if (pci_read_byte(dev, 0x6d) != new) {
 		msg_pinfo("tried to set 0x%x to 0x%x on %s failed (WARNING ONLY)\n", 0x6d, new, name);
@@ -931,7 +919,7 @@
 #if 0
 	val |= (1 << 6);
 	val &= ~(1 << 5);
-	pci_write_byte(dev, 0x8a, val);
+	rpci_write_byte(dev, 0x8a, val);
 #endif
 
 	if (mcp6x_spi_init(want_spi)) {
@@ -954,11 +942,11 @@
 	/* Set the 4MB enable bit. */
 	val = pci_read_byte(dev, 0x41);
 	val |= 0x0e;
-	pci_write_byte(dev, 0x41, val);
+	rpci_write_byte(dev, 0x41, val);
 
 	val = pci_read_byte(dev, 0x43);
 	val |= (1 << 4);
-	pci_write_byte(dev, 0x43, val);
+	rpci_write_byte(dev, 0x43, val);
 
 	return 0;
 }
diff --git a/drkaiser.c b/drkaiser.c
index 5f5e580..984dbdd 100644
--- a/drkaiser.c
+++ b/drkaiser.c
@@ -47,7 +47,7 @@
 			   drkaiser_pcidev);
 
 	/* Write magic register to enable flash write. */
-	pci_write_word(pcidev_dev, PCI_MAGIC_DRKAISER_ADDR,
+	rpci_write_word(pcidev_dev, PCI_MAGIC_DRKAISER_ADDR,
 		       PCI_MAGIC_DRKAISER_VALUE);
 
 	/* Map 128KB flash memory window. */
@@ -61,8 +61,7 @@
 
 int drkaiser_shutdown(void)
 {
-	/* Write protect the flash again. */
-	pci_write_word(pcidev_dev, PCI_MAGIC_DRKAISER_ADDR, 0);
+	/* Flash write is disabled automatically by PCI restore. */
 	pci_cleanup(pacc);
 	release_io_perms();
 	return 0;
diff --git a/flashrom.c b/flashrom.c
index b45ac13..64c3fe5 100644
--- a/flashrom.c
+++ b/flashrom.c
@@ -442,7 +442,7 @@
 	{}, /* This entry corresponds to PROGRAMMER_INVALID. */
 };
 
-#define SHUTDOWN_MAXFN 4
+#define SHUTDOWN_MAXFN 32
 static int shutdown_fn_count = 0;
 struct shutdown_func_data {
 	void (*func) (void *data);
diff --git a/gfxnvidia.c b/gfxnvidia.c
index 7825380..b6dc5f4 100644
--- a/gfxnvidia.c
+++ b/gfxnvidia.c
@@ -75,7 +75,7 @@
 	/* Allow access to flash interface (will disable screen). */
 	reg32 = pci_read_long(pcidev_dev, 0x50);
 	reg32 &= ~(1 << 0);
-	pci_write_long(pcidev_dev, 0x50, reg32);
+	rpci_write_long(pcidev_dev, 0x50, reg32);
 
 	nvidia_bar = physmap("NVIDIA", io_base_addr, 16 * 1024 * 1024);
 
@@ -89,13 +89,9 @@
 
 int gfxnvidia_shutdown(void)
 {
-	uint32_t reg32;
-
-	/* Disallow access to flash interface (and re-enable screen). */
-	reg32 = pci_read_long(pcidev_dev, 0x50);
-	reg32 |= (1 << 0);
-	pci_write_long(pcidev_dev, 0x50, reg32);
-
+	/* Flash interface access is disabled (and screen enabled) automatically
+	 * by PCI restore.
+	 */
 	pci_cleanup(pacc);
 	release_io_perms();
 	return 0;
diff --git a/pcidev.c b/pcidev.c
index 4b16db2..b569df4 100644
--- a/pcidev.c
+++ b/pcidev.c
@@ -142,3 +142,73 @@
 		       (devs[i].status == NT) ? " (untested)" : "");
 	}
 }
+
+enum pci_write_type {
+	pci_write_type_byte,
+	pci_write_type_word,
+	pci_write_type_long,
+};
+
+struct undo_pci_write_data {
+	struct pci_dev dev;
+	int reg;
+	enum pci_write_type type;
+	union {
+		uint8_t bytedata;
+		uint16_t worddata;
+		uint32_t longdata;
+	};
+};
+
+void undo_pci_write(void *p)
+{
+	struct undo_pci_write_data *data = p;
+	msg_pdbg("Restoring PCI config space for %02x:%02x:%01x reg 0x%02x\n",
+		 data->dev.bus, data->dev.dev, data->dev.func, data->reg);
+	switch (data->type) {
+	case pci_write_type_byte:
+		pci_write_byte(&data->dev, data->reg, data->bytedata);
+		break;
+	case pci_write_type_word:
+		pci_write_word(&data->dev, data->reg, data->worddata);
+		break;
+	case pci_write_type_long:
+		pci_write_long(&data->dev, data->reg, data->longdata);
+		break;
+	}
+	/* p was allocated in register_undo_pci_write. */
+	free(p);
+}
+
+#define register_undo_pci_write(a, b, c) 				\
+{									\
+	struct undo_pci_write_data *undo_pci_write_data;		\
+	undo_pci_write_data = malloc(sizeof(struct undo_pci_write_data)); \
+	undo_pci_write_data->dev = *a;					\
+	undo_pci_write_data->reg = b;					\
+	undo_pci_write_data->type = pci_write_type_##c;			\
+	undo_pci_write_data->c##data = pci_read_##c(dev, reg);		\
+	register_shutdown(undo_pci_write, undo_pci_write_data);		\
+}
+
+#define register_undo_pci_write_byte(a, b) register_undo_pci_write(a, b, byte)
+#define register_undo_pci_write_word(a, b) register_undo_pci_write(a, b, word)
+#define register_undo_pci_write_long(a, b) register_undo_pci_write(a, b, long)
+
+int rpci_write_byte(struct pci_dev *dev, int reg, uint8_t data)
+{
+	register_undo_pci_write_byte(dev, reg);
+	return pci_write_byte(dev, reg, data);
+}
+ 
+int rpci_write_word(struct pci_dev *dev, int reg, uint16_t data)
+{
+	register_undo_pci_write_word(dev, reg);
+	return pci_write_word(dev, reg, data);
+}
+ 
+int rpci_write_long(struct pci_dev *dev, int reg, uint32_t data)
+{
+	register_undo_pci_write_long(dev, reg);
+	return pci_write_long(dev, reg, data);
+}
diff --git a/programmer.h b/programmer.h
index 6407695..ed9d0b2 100644
--- a/programmer.h
+++ b/programmer.h
@@ -212,6 +212,12 @@
 };
 uint32_t pcidev_validate(struct pci_dev *dev, uint32_t bar, const struct pcidev_status *devs);
 uint32_t pcidev_init(uint16_t vendor_id, uint32_t bar, const struct pcidev_status *devs);
+/* rpci_write_* are reversible writes. The original PCI config space register
+ * contents will be restored on shutdown.
+ */
+int rpci_write_byte(struct pci_dev *dev, int reg, u8 data);
+int rpci_write_word(struct pci_dev *dev, int reg, u16 data);
+int rpci_write_long(struct pci_dev *dev, int reg, u32 data);
 #endif
 
 /* print.c */