pciehp: Fix wrong slot control register access

Current pciehp implementaion clears hotplug events without waiting for
command completion. Because of this, events might not be cleared properly.
To prevent this problem, we must use pciehp_write_cmd() to write to
Slot Control register.

Signed-off-by: Kenji Kaneshige <[e-mail]>
Signed-off-by: Kristen Carlson Accardi <[e-mail]>
Signed-off-by: Jesse Barnes <[e-mail]>

drivers/pci/hotplug/pciehp_hpc.c

···
 
 /**
  * pcie_write_cmd - Issue controller command
- * @slot: slot to which the command is issued
+ * @ctrl: controller to which the command is issued
  * @cmd:  command value written to slot control register
  * @mask: bitmask of slot control register to be modified
  */
-static int pcie_write_cmd(struct slot *slot, u16 cmd, u16 mask)
+static int pcie_write_cmd(struct controller *ctrl, u16 cmd, u16 mask)
 {
-	struct controller *ctrl = slot->ctrl;
 	int retval = 0;
 	u16 slot_status;
 	u16 slot_ctrl;
···
 		cmd_mask = cmd_mask | HP_INTR_ENABLE;
 	}
 
-	rc = pcie_write_cmd(slot, slot_cmd, cmd_mask);
+	rc = pcie_write_cmd(slot->ctrl, slot_cmd, cmd_mask);
 	slot->last_emi_toggle = get_seconds();
 
 	return rc;
···
 		cmd_mask = cmd_mask | HP_INTR_ENABLE;
 	}
 
-	rc = pcie_write_cmd(slot, slot_cmd, cmd_mask);
+	rc = pcie_write_cmd(ctrl, slot_cmd, cmd_mask);
 	dbg("%s: SLOTCTRL %x write cmd %x\n",
 	    __func__, ctrl->cap_base + SLOTCTRL, slot_cmd);
 
···
 		cmd_mask = cmd_mask | HP_INTR_ENABLE;
 	}
 
-	pcie_write_cmd(slot, slot_cmd, cmd_mask);
+	pcie_write_cmd(ctrl, slot_cmd, cmd_mask);
 
 	dbg("%s: SLOTCTRL %x write cmd %x\n",
 	    __func__, ctrl->cap_base + SLOTCTRL, slot_cmd);
···
 		cmd_mask = cmd_mask | HP_INTR_ENABLE;
 	}
 
-	pcie_write_cmd(slot, slot_cmd, cmd_mask);
+	pcie_write_cmd(ctrl, slot_cmd, cmd_mask);
 	dbg("%s: SLOTCTRL %x write cmd %x\n",
 	    __func__, ctrl->cap_base + SLOTCTRL, slot_cmd);
 }
···
 		cmd_mask = cmd_mask | HP_INTR_ENABLE;
 	}
 
-	pcie_write_cmd(slot, slot_cmd, cmd_mask);
+	pcie_write_cmd(ctrl, slot_cmd, cmd_mask);
 
 	dbg("%s: SLOTCTRL %x write cmd %x\n",
 	    __func__, ctrl->cap_base + SLOTCTRL, slot_cmd);
···
 		           HP_INTR_ENABLE;
 	}
 
-	retval = pcie_write_cmd(slot, slot_cmd, cmd_mask);
+	retval = pcie_write_cmd(ctrl, slot_cmd, cmd_mask);
 
 	if (retval) {
 		err("%s: Write %x command failed!\n", __func__, slot_cmd);
···
 			   HP_INTR_ENABLE;
 	}
 
-	retval = pcie_write_cmd(slot, slot_cmd, cmd_mask);
+	retval = pcie_write_cmd(ctrl, slot_cmd, cmd_mask);
 	if (retval) {
 		err("%s: Write command failed!\n", __func__);
 		retval = -1;
···
 static int pcie_init_hardware_part1(struct controller *ctrl,
 				    struct pcie_device *dev)
 {
-	int rc;
-	u16 temp_word;
-	u32 slot_cap;
-	u16 slot_status;
-
-	rc = pciehp_readl(ctrl, SLOTCAP, &slot_cap);
-	if (rc) {
-		err("%s: Cannot read SLOTCAP register\n", __func__);
-		return -1;
-	}
-
 	/* Mask Hot-plug Interrupt Enable */
-	rc = pciehp_readw(ctrl, SLOTCTRL, &temp_word);
-	if (rc) {
-		err("%s: Cannot read SLOTCTRL register\n", __func__);
-		return -1;
-	}
-
-	dbg("%s: SLOTCTRL %x value read %x\n",
-	    __func__, ctrl->cap_base + SLOTCTRL, temp_word);
-	temp_word = (temp_word & ~HP_INTR_ENABLE & ~CMD_CMPL_INTR_ENABLE) |
-		0x00;
-
-	rc = pciehp_writew(ctrl, SLOTCTRL, temp_word);
-	if (rc) {
-		err("%s: Cannot write to SLOTCTRL register\n", __func__);
-		return -1;
-	}
-
-	rc = pciehp_readw(ctrl, SLOTSTATUS, &slot_status);
-	if (rc) {
-		err("%s: Cannot read SLOTSTATUS register\n", __func__);
-		return -1;
-	}
-
-	temp_word = 0x1F; /* Clear all events */
-	rc = pciehp_writew(ctrl, SLOTSTATUS, temp_word);
-	if (rc) {
-		err("%s: Cannot write to SLOTSTATUS register\n", __func__);
+	if (pcie_write_cmd(ctrl, 0, HP_INTR_ENABLE | CMD_CMPL_INTR_ENABLE)) {
+		err("%s: Cannot mask hotplug interrupt enable\n", __func__);
 		return -1;
 	}
 	return 0;
···
 
 int pcie_init_hardware_part2(struct controller *ctrl, struct pcie_device *dev)
 {
-	int rc;
-	u16 temp_word;
-	u16 intr_enable = 0;
-	u32 slot_cap;
-	u16 slot_status;
-
-	rc = pciehp_readw(ctrl, SLOTCTRL, &temp_word);
-	if (rc) {
-		err("%s: Cannot read SLOTCTRL register\n", __func__);
-		goto abort;
-	}
-
-	intr_enable = intr_enable | PRSN_DETECT_ENABLE;
-
-	rc = pciehp_readl(ctrl, SLOTCAP, &slot_cap);
-	if (rc) {
-		err("%s: Cannot read SLOTCAP register\n", __func__);
-		goto abort;
-	}
-
-	if (ATTN_BUTTN(slot_cap))
-		intr_enable = intr_enable | ATTN_BUTTN_ENABLE;
-
-	if (POWER_CTRL(slot_cap))
-		intr_enable = intr_enable | PWR_FAULT_DETECT_ENABLE;
-
-	if (MRL_SENS(slot_cap))
-		intr_enable = intr_enable | MRL_DETECT_ENABLE;
-
-	temp_word = (temp_word & ~intr_enable) | intr_enable;
-
-	if (pciehp_poll_mode) {
-		temp_word = (temp_word & ~HP_INTR_ENABLE) | 0x0;
-	} else {
-		temp_word = (temp_word & ~HP_INTR_ENABLE) | HP_INTR_ENABLE;
-	}
+	u16 cmd, mask;
 
 	/*
-	 * Unmask Hot-plug Interrupt Enable for the interrupt
-	 * notification mechanism case.
+	 * We need to clear all events before enabling hotplug interrupt
+	 * notification mechanism in order for hotplug controler to
+	 * generate interrupts.
 	 */
-	rc = pciehp_writew(ctrl, SLOTCTRL, temp_word);
-	if (rc) {
-		err("%s: Cannot write to SLOTCTRL register\n", __func__);
+	if (pciehp_writew(ctrl, SLOTSTATUS, 0x1f)) {
+		err("%s: Cannot write to SLOTSTATUS register\n", __FUNCTION__);
+		return -1;
+	}
+
+	cmd = PRSN_DETECT_ENABLE;
+	if (ATTN_BUTTN(ctrl->ctrlcap))
+		cmd |= ATTN_BUTTN_ENABLE;
+	if (POWER_CTRL(ctrl->ctrlcap))
+		cmd |= PWR_FAULT_DETECT_ENABLE;
+	if (MRL_SENS(ctrl->ctrlcap))
+		cmd |= MRL_DETECT_ENABLE;
+	if (!pciehp_poll_mode)
+		cmd |= HP_INTR_ENABLE;
+
+	mask = PRSN_DETECT_ENABLE | ATTN_BUTTN_ENABLE |
+		PWR_FAULT_DETECT_ENABLE | MRL_DETECT_ENABLE | HP_INTR_ENABLE;
+
+	if (pcie_write_cmd(ctrl, cmd, mask)) {
+		err("%s: Cannot enable software notification\n", __func__);
 		goto abort;
 	}
-	rc = pciehp_readw(ctrl, SLOTSTATUS, &slot_status);
-	if (rc) {
-		err("%s: Cannot read SLOTSTATUS register\n", __func__);
-		goto abort_disable_intr;
-	}
 
-	temp_word =  0x1F; /* Clear all events */
-	rc = pciehp_writew(ctrl, SLOTSTATUS, temp_word);
-	if (rc) {
-		err("%s: Cannot write to SLOTSTATUS register\n", __func__);
-		goto abort_disable_intr;
-	}
-
-	if (pciehp_force) {
+	if (pciehp_force)
 		dbg("Bypassing BIOS check for pciehp use on %s\n",
 				pci_name(ctrl->pci_dev));
-	} else {
-		rc = pciehp_get_hp_hw_control_from_firmware(ctrl->pci_dev);
-		if (rc)
-			goto abort_disable_intr;
-	}
+	else if (pciehp_get_hp_hw_control_from_firmware(ctrl->pci_dev))
+		goto abort_disable_intr;
 
 	return 0;
 
 	/* We end up here for the many possible ways to fail this API. */
 abort_disable_intr:
-	rc = pciehp_readw(ctrl, SLOTCTRL, &temp_word);
-	if (!rc) {
-		temp_word &= ~(intr_enable | HP_INTR_ENABLE);
-		rc = pciehp_writew(ctrl, SLOTCTRL, temp_word);
-	}
-	if (rc)
+	if (pcie_write_cmd(ctrl, 0, HP_INTR_ENABLE))
 		err("%s : disabling interrupts failed\n", __func__);
 abort:
 	return -1;