[PATCH v3 5/5] fsl_msi: add removal path and probe failing path

[Li Yang <leoli@freescale.com>]

Also cleanup the probe function.

Signed-off-by: Li Yang <leoli@freescale.com>
---
 arch/powerpc/sysdev/fsl_msi.c |   36 ++++++++++++++++++++++++++++++------
 arch/powerpc/sysdev/fsl_msi.h |    1 +
 2 files changed, 31 insertions(+), 6 deletions(-)

diff --git a/arch/powerpc/sysdev/fsl_msi.c b/arch/powerpc/sysdev/fsl_msi.c
index 7c442e2..025aa9b 100644
--- a/arch/powerpc/sysdev/fsl_msi.c
+++ b/arch/powerpc/sysdev/fsl_msi.c
@@ -249,6 +249,30 @@ unlock:
 	spin_unlock(&desc->lock);
 }
 
+static int fsl_of_msi_remove(struct of_device *ofdev)
+{
+	struct fsl_msi *msi = ofdev->dev.platform_data;
+	int virq, i;
+	struct fsl_msi_cascade_data *cascade_data;
+
+	if (msi->list.prev != NULL)
+		list_del(&msi->list);
+	for (i = 0; i < NR_MSI_REG; i++) {
+		virq = msi->msi_virqs[i];
+		if (virq != NO_IRQ) {
+			cascade_data = get_irq_data(virq);
+			kfree(cascade_data);
+			irq_dispose_mapping(virq);
+		}
+	}
+	if (msi->bitmap.bitmap)
+		msi_bitmap_free(&msi->bitmap);
+	iounmap(msi->msi_regs);
+	kfree(msi);
+
+	return 0;
+}
+
 static int __devinit fsl_of_msi_probe(struct of_device *dev,
 				const struct of_device_id *match)
 {
@@ -268,9 +292,9 @@ static int __devinit fsl_of_msi_probe(struct of_device *dev,
 	msi = kzalloc(sizeof(struct fsl_msi), GFP_KERNEL);
 	if (!msi) {
 		dev_err(&dev->dev, "No memory for MSI structure\n");
-		err = -ENOMEM;
-		goto error_out;
+		return -ENOMEM;
 	}
+	dev->dev.platform_data = msi;
 
 	msi->irqhost = irq_alloc_host(dev->node, IRQ_HOST_MAP_LINEAR,
 				      NR_MSI_IRQS, &fsl_msi_host_ops, 0);
@@ -327,9 +351,7 @@ static int __devinit fsl_of_msi_probe(struct of_device *dev,
 		offset = *p / IRQS_PER_MSI_REG;
 
 	count /= sizeof(u32);
-	for (i = 0; i < count / 2; i++) {
-		if (i > NR_MSI_REG)
-			break;
+	for (i = 0; i < min(count / 2, NR_MSI_REG); i++) {
 		virt_msir = irq_of_parse_and_map(dev->node, i);
 		if (virt_msir != NO_IRQ) {
 			cascade_data = kzalloc(
@@ -341,6 +363,7 @@ static int __devinit fsl_of_msi_probe(struct of_device *dev,
 				err = -ENOMEM;
 				goto error_out;
 			}
+			msi->msi_virqs[i] = virt_msir;
 			cascade_data->index = i + offset;
 			cascade_data->msi_data = msi;
 			set_irq_data(virt_msir, (void *)cascade_data);
@@ -362,7 +385,7 @@ static int __devinit fsl_of_msi_probe(struct of_device *dev,
 	}
 	return 0;
 error_out:
-	kfree(msi);
+	fsl_of_msi_remove(dev);
 	return err;
 }
 
@@ -392,6 +415,7 @@ static struct of_platform_driver fsl_of_msi_driver = {
 	.name = "fsl-msi",
 	.match_table = fsl_of_msi_ids,
 	.probe = fsl_of_msi_probe,
+	.remove = fsl_of_msi_remove,
 };
 
 static __init int fsl_of_msi_init(void)
diff --git a/arch/powerpc/sysdev/fsl_msi.h b/arch/powerpc/sysdev/fsl_msi.h
index 8fc5523..624580c 100644
--- a/arch/powerpc/sysdev/fsl_msi.h
+++ b/arch/powerpc/sysdev/fsl_msi.h
@@ -32,6 +32,7 @@ struct fsl_msi {
 	u32 msi_addr_hi;
 	void __iomem *msi_regs;
 	u32 feature;
+	int msi_virqs[NR_MSI_REG];
 
 	struct msi_bitmap bitmap;
 
-- 
1.6.6-rc1.GIT