Added error handling to probe()

diff --git a/motorek.c b/motorek.c
index 205297f7b8818d20c3648d540f235781224fb625..6be72fcd92cb1bdce1aa779bd83f9a03045096bd 100644 (file)
--- a/motorek.c
+++ b/motorek.c
@@ -150,19 +150,23 @@ static int __devinit motorek_probe(struct of_device* dev,
        if (!m)
                return -ENOMEM;
 
+       err = -ENODEV;
+       
        m->pwmf = iomap_gpt_by_phandle(of_get_property(dn, "pwmf", NULL));
+       if (!m->pwmf)
+               goto err_free;
+       
        m->pwmb = iomap_gpt_by_phandle(of_get_property(dn, "pwmb", NULL));
+       if (!m->pwmb)
+               goto err_unmapf;
        
-       if (!m->pwmf || !m->pwmb) {
-               printk(KERN_ERR "%s() mmap failed\n", __func__);
-               return -ENXIO;
-       }
-
        m->irq = irq_of_parse_and_map(dn, 0);
+       if (m->irq == NO_IRQ)
+               goto err_unmapb;
 
        err = request_irq(m->irq, motorek_irq, 0, "motorek", m);
        if (err)
-               return err;
+               goto err_unmapb;
 
        motorek_init(m);
 
@@ -170,26 +174,35 @@ static int __devinit motorek_probe(struct of_device* dev,
 
        err = device_create_file(&dev->dev,&dev_attr_position);
        if (err)
-               return err;
+               goto err_irq;
        err = device_create_file(&dev->dev,&dev_attr_action);
        if (err)
-               return err;
+               goto err_irq;
 
        printk(KERN_NOTICE "Motorek initialized\n");
 
        return 0;
+
+err_irq:
+       free_irq(m->irq, m);
+err_unmapb:
+       iounmap(m->pwmb);
+err_unmapf:
+       iounmap(m->pwmf);
+err_free:
+       kfree(m);
+       return err;
 }
 
 static int __devexit motorek_remove(struct of_device* dev)
 {
        struct motorek *m;
-       m = platform_get_drvdata(dev);
 
        printk(KERN_NOTICE "Removing motorek\n");
-
+       m = platform_get_drvdata(dev);
        free_irq(m->irq, m);
-
        motorek_done(m);
+       kfree(m);
        return 0;
 }
 
@@ -223,7 +236,6 @@ static int __init motorek_init_module(void)
        ret = of_register_platform_driver(&motorek_driver);
        return ret;
 }
-module_init(motorek_init_module);
 
 static void __exit motorek_exit_module(void)
 {
@@ -237,6 +249,8 @@ static void __exit motorek_exit_module(void)
        }
        of_unregister_platform_driver(&motorek_driver);
 }
+
+module_init(motorek_init_module);
 module_exit(motorek_exit_module);