usb: isp1760: Refactor PCI initialization code

Move the initialization code out of the PCI probe function to a new
function in order to simplify and fix error handling.

Signed-off-by: Laurent Pinchart <laurent.pinchart@ideasonboard.com>
Signed-off-by: Felipe Balbi <balbi@ti.com>

diff --git a/drivers/usb/host/isp1760-if.c b/drivers/usb/host/isp1760-if.c
index dbb287ca9d82d71a947561fb1f1c5e004875203a..7c03320f7831cfca2eb6a95eb765bfab7c499105 100644 (file)
--- a/drivers/usb/host/isp1760-if.c
+++ b/drivers/usb/host/isp1760-if.c
@@ -25,69 +25,34 @@
 #endif
 
 #ifdef CONFIG_PCI
-static int isp1761_pci_probe(struct pci_dev *dev,
-               const struct pci_device_id *id)
+static int isp1761_pci_init(struct pci_dev *dev)
 {
+       resource_size_t mem_start;
+       resource_size_t mem_length;
+       u8 __iomem *iobase;
        u8 latency, limit;
-       __u32 reg_data;
        int retry_count;
-       unsigned int devflags = 0;
-       int ret_status = 0;
-
-       resource_size_t pci_mem_phy0;
-       resource_size_t memlength;
+       u32 reg_data;
 
-       u8 __iomem *chip_addr;
-       u8 __iomem *iobase;
-       resource_size_t nxp_pci_io_base;
-       resource_size_t iolength;
-
-       if (usb_disabled())
-               return -ENODEV;
-
-       if (pci_enable_device(dev) < 0)
-               return -ENODEV;
-
-       if (!dev->irq)
-               return -ENODEV;
-
-       /* Grab the PLX PCI mem maped port start address we need  */
-       nxp_pci_io_base = pci_resource_start(dev, 0);
-       iolength = pci_resource_len(dev, 0);
-
-       if (!request_mem_region(nxp_pci_io_base, iolength, "ISP1761 IO MEM")) {
-               printk(KERN_ERR "request region #1\n");
-               return -EBUSY;
-       }
-
-       iobase = ioremap_nocache(nxp_pci_io_base, iolength);
-       if (!iobase) {
-               printk(KERN_ERR "ioremap #1\n");
-               ret_status = -ENOMEM;
-               goto cleanup1;
-       }
        /* Grab the PLX PCI shared memory of the ISP 1761 we need  */
-       pci_mem_phy0 = pci_resource_start(dev, 3);
-       memlength = pci_resource_len(dev, 3);
-       if (memlength < 0xffff) {
+       mem_start = pci_resource_start(dev, 3);
+       mem_length = pci_resource_len(dev, 3);
+       if (mem_length < 0xffff) {
                printk(KERN_ERR "memory length for this resource is wrong\n");
-               ret_status = -ENOMEM;
-               goto cleanup2;
+               return -ENOMEM;
        }
 
-       if (!request_mem_region(pci_mem_phy0, memlength, "ISP-PCI")) {
+       if (!request_mem_region(mem_start, mem_length, "ISP-PCI")) {
                printk(KERN_ERR "host controller already in use\n");
-               ret_status = -EBUSY;
-               goto cleanup2;
+               return -EBUSY;
        }
 
        /* map available memory */
-       chip_addr = ioremap_nocache(pci_mem_phy0,memlength);
-       if (!chip_addr) {
+       iobase = ioremap_nocache(mem_start, mem_length);
+       if (!iobase) {
                printk(KERN_ERR "Error ioremap failed\n");
-               release_mem_region(pci_mem_phy0, memlength);
-               ret_status = -ENOMEM;
-               goto cleanup2;
+               release_mem_region(mem_start, mem_length);
+               return -ENOMEM;
        }
 
        /* bad pci latencies can contribute to overruns */
@@ -108,25 +73,38 @@ static int isp1761_pci_probe(struct pci_dev *dev,
                /*by default host is in 16bit mode, so
                 * io operations at this stage must be 16 bit
                 * */
-               writel(0xface, chip_addr + HC_SCRATCH_REG);
+               writel(0xface, iobase + HC_SCRATCH_REG);
                udelay(100);
-               reg_data = readl(chip_addr + HC_SCRATCH_REG) & 0x0000ffff;
+               reg_data = readl(iobase + HC_SCRATCH_REG) & 0x0000ffff;
                retry_count--;
        }
 
-       iounmap(chip_addr);
-       release_mem_region(pci_mem_phy0, memlength);
+       iounmap(iobase);
+       release_mem_region(mem_start, mem_length);
 
        /* Host Controller presence is detected by writing to scratch register
         * and reading back and checking the contents are same or not
         */
        if (reg_data != 0xFACE) {
                dev_err(&dev->dev, "scratch register mismatch %x\n", reg_data);
-               ret_status = -ENOMEM;
-               goto cleanup2;
+               return -ENOMEM;
        }
 
-       pci_set_master(dev);
+       /* Grab the PLX PCI mem maped port start address we need  */
+       mem_start = pci_resource_start(dev, 0);
+       mem_length = pci_resource_len(dev, 0);
+
+       if (!request_mem_region(mem_start, mem_length, "ISP1761 IO MEM")) {
+               printk(KERN_ERR "request region #1\n");
+               return -EBUSY;
+       }
+
+       iobase = ioremap_nocache(mem_start, mem_length);
+       if (!iobase) {
+               printk(KERN_ERR "ioremap #1\n");
+               release_mem_region(mem_start, mem_length);
+               return -ENOMEM;
+       }
 
        /* configure PLX PCI chip to pass interrupts */
 #define PLX_INT_CSR_REG 0x68
@@ -136,17 +114,43 @@ static int isp1761_pci_probe(struct pci_dev *dev,
 
        /* done with PLX IO access */
        iounmap(iobase);
-       release_mem_region(nxp_pci_io_base, iolength);
+       release_mem_region(mem_start, mem_length);
+
+       return 0;
+}
+
+static int isp1761_pci_probe(struct pci_dev *dev,
+               const struct pci_device_id *id)
+{
+       unsigned int devflags = 0;
+       int ret;
+
+       if (usb_disabled())
+               return -ENODEV;
+
+       if (!dev->irq)
+               return -ENODEV;
+
+       if (pci_enable_device(dev) < 0)
+               return -ENODEV;
+
+       ret = isp1761_pci_init(dev);
+       if (ret < 0)
+               goto error;
+
+       pci_set_master(dev);
 
        dev->dev.dma_mask = NULL;
-       return isp1760_register(&dev->resource[3], dev->irq, IRQF_SHARED,
-                               &dev->dev, devflags);
+       ret = isp1760_register(&dev->resource[3], dev->irq, IRQF_SHARED,
+                              &dev->dev, devflags);
+       if (ret < 0)
+               goto error;
 
-cleanup2:
-       iounmap(iobase);
-cleanup1:
-       release_mem_region(nxp_pci_io_base, iolength);
-       return ret_status;
+       return 0;
+
+error:
+       pci_disable_device(dev);
+       return ret;
 }
 
 static void isp1761_pci_remove(struct pci_dev *dev)