1 files changed, 54 insertions, 28 deletions

diff --git a/drivers/pci/hotplug/shpchp_hpc.c b/drivers/pci/hotplug/shpchp_hpc.c
index 4d8aee119134..83a5226ba9ed 100644
--- a/drivers/pci/hotplug/shpchp_hpc.c
+++ b/drivers/pci/hotplug/shpchp_hpc.c
@@ -302,21 +302,51 @@ static void start_int_poll_timer(struct php_ctlr_state_s *php_ctlr, int sec)
         add_timer(&php_ctlr->int_poll_timer);
 }
 
+static inline int is_ctrl_busy(struct controller *ctrl)
+{
+        u16 cmd_status = shpc_readw(ctrl, CMD_STATUS);
+        return cmd_status & 0x1;
+}
+
+/*
+ * Returns 1 if SHPC finishes executing a command within 1 sec,
+ * otherwise returns 0.
+ */
+static inline int shpc_poll_ctrl_busy(struct controller *ctrl)
+{
+        int i;
+
+        if (!is_ctrl_busy(ctrl))
+                return 1;
+
+        /* Check every 0.1 sec for a total of 1 sec */
+        for (i = 0; i < 10; i++) {
+                msleep(100);
+                if (!is_ctrl_busy(ctrl))
+                        return 1;
+        }
+
+        return 0;
+}
+
 static inline int shpc_wait_cmd(struct controller *ctrl)
 {
         int retval = 0;
-        unsigned int timeout_msec = shpchp_poll_mode ? 2000 : 1000;
-        unsigned long timeout = msecs_to_jiffies(timeout_msec);
-        int rc = wait_event_interruptible_timeout(ctrl->queue,
-                                                  !ctrl->cmd_busy, timeout);
-        if (!rc) {
+        unsigned long timeout = msecs_to_jiffies(1000);
+        int rc;
+
+        if (shpchp_poll_mode)
+                rc = shpc_poll_ctrl_busy(ctrl);
+        else
+                rc = wait_event_interruptible_timeout(ctrl->queue,
+                                                !is_ctrl_busy(ctrl), timeout);
+        if (!rc && is_ctrl_busy(ctrl)) {
                 retval = -EIO;
-                err("Command not completed in %d msec\n", timeout_msec);
+                err("Command not completed in 1000 msec\n");
         } else if (rc < 0) {
                 retval = -EINTR;
                 info("Command was interrupted by a signal\n");
         }
-        ctrl->cmd_busy = 0;
 
         return retval;
 }
@@ -327,26 +357,15 @@ static int shpc_write_cmd(struct slot *slot, u8 t_slot, u8 cmd)
         u16 cmd_status;
         int retval = 0;
         u16 temp_word;
-        int i;
 
         DBG_ENTER_ROUTINE 
 
         mutex_lock(&slot->ctrl->cmd_lock);
 
-        for (i = 0; i < 10; i++) {
-                cmd_status = shpc_readw(ctrl, CMD_STATUS);
-                
-                if (!(cmd_status & 0x1))
-                        break;
-                /*  Check every 0.1 sec for a total of 1 sec*/
-                msleep(100);
-        }
-
-        cmd_status = shpc_readw(ctrl, CMD_STATUS);
-        
-        if (cmd_status & 0x1) { 
+        if (!shpc_poll_ctrl_busy(ctrl)) {
                 /* After 1 sec and and the controller is still busy */
-                err("%s : Controller is still busy after 1 sec.\n", __FUNCTION__);
+                err("%s : Controller is still busy after 1 sec.\n",
+                    __FUNCTION__);
                 retval = -EBUSY;
                 goto out;
         }
@@ -358,7 +377,6 @@ static int shpc_write_cmd(struct slot *slot, u8 t_slot, u8 cmd)
         /* To make sure the Controller Busy bit is 0 before we send out the
          * command. 
          */
-        slot->ctrl->cmd_busy = 1;
         shpc_writew(ctrl, CMD, temp_word);
 
         /*
@@ -908,7 +926,6 @@ static irqreturn_t shpc_isr(int irq, void *dev_id)
                 serr_int &= ~SERR_INTR_RSVDZ_MASK;
                 shpc_writel(ctrl, SERR_INTR_ENABLE, serr_int);
 
-                ctrl->cmd_busy = 0;
                 wake_up_interruptible(&ctrl->queue);
         }
 
@@ -1101,7 +1118,7 @@ int shpc_init(struct controller * ctrl, struct pci_dev * pdev)
 {
         struct php_ctlr_state_s *php_ctlr, *p;
         void *instance_id = ctrl;
-        int rc, num_slots = 0;
+        int rc = -1, num_slots = 0;
         u8 hp_slot;
         u32 shpc_base_offset;
         u32 tempdword, slot_reg, slot_config;
@@ -1167,11 +1184,15 @@ int shpc_init(struct controller * ctrl, struct pci_dev * pdev)
         info("HPC vendor_id %x device_id %x ss_vid %x ss_did %x\n", pdev->vendor, pdev->device, pdev->subsystem_vendor, 
                 pdev->subsystem_device);
         
-        if (pci_enable_device(pdev))
+        rc = pci_enable_device(pdev);
+        if (rc) {
+                err("%s: pci_enable_device failed\n", __FUNCTION__);
                 goto abort_free_ctlr;
+        }
 
         if (!request_mem_region(ctrl->mmio_base, ctrl->mmio_size, MY_NAME)) {
                 err("%s: cannot reserve MMIO region\n", __FUNCTION__);
+                rc = -1;
                 goto abort_free_ctlr;
         }
 
@@ -1180,6 +1201,7 @@ int shpc_init(struct controller * ctrl, struct pci_dev * pdev)
                 err("%s: cannot remap MMIO region %lx @ %lx\n", __FUNCTION__,
                     ctrl->mmio_size, ctrl->mmio_base);
                 release_mem_region(ctrl->mmio_base, ctrl->mmio_size);
+                rc = -1;
                 goto abort_free_ctlr;
         }
         dbg("%s: php_ctlr->creg %p\n", __FUNCTION__, php_ctlr->creg);
@@ -1282,8 +1304,10 @@ int shpc_init(struct controller * ctrl, struct pci_dev * pdev)
          */
         if (atomic_add_return(1, &shpchp_num_controllers) == 1) {
                 shpchp_wq = create_singlethread_workqueue("shpchpd");
-                if (!shpchp_wq)
-                        return -ENOMEM;
+                if (!shpchp_wq) {
+                        rc = -ENOMEM;
+                        goto abort_free_ctlr;
+                }
         }
 
         /*
@@ -1313,8 +1337,10 @@ int shpc_init(struct controller * ctrl, struct pci_dev * pdev)
 
         /* We end up here for the many possible ways to fail this API.  */
 abort_free_ctlr:
+        if (php_ctlr->creg)
+                iounmap(php_ctlr->creg);
         kfree(php_ctlr);
 abort:
         DBG_LEAVE_ROUTINE
-        return -1;
+        return rc;
 }