2 files changed, 52 insertions, 46 deletions

diff --git a/drivers/leds/leds-fsg.c b/drivers/leds/leds-fsg.c
index be0e12144b8b..34935155c1c0 100644
--- a/drivers/leds/leds-fsg.c
+++ b/drivers/leds/leds-fsg.c
@@ -161,6 +161,16 @@ static int fsg_led_probe(struct platform_device *pdev)
 {
         int ret;
 
+        /* Map the LED chip select address space */
+        latch_address = (unsigned short *) ioremap(IXP4XX_EXP_BUS_BASE(2), 512);
+        if (!latch_address) {
+                ret = -ENOMEM;
+                goto failremap;
+        }
+
+        latch_value = 0xffff;
+        *latch_address = latch_value;
+
         ret = led_classdev_register(&pdev->dev, &fsg_wlan_led);
         if (ret < 0)
                 goto failwlan;
@@ -185,20 +195,8 @@ static int fsg_led_probe(struct platform_device *pdev)
         if (ret < 0)
                 goto failring;
 
-        /* Map the LED chip select address space */
-        latch_address = (unsigned short *) ioremap(IXP4XX_EXP_BUS_BASE(2), 512);
-        if (!latch_address) {
-                ret = -ENOMEM;
-                goto failremap;
-        }
-
-        latch_value = 0xffff;
-        *latch_address = latch_value;
-
         return ret;
 
- failremap:
-        led_classdev_unregister(&fsg_ring_led);
  failring:
         led_classdev_unregister(&fsg_sync_led);
  failsync:
@@ -210,14 +208,14 @@ static int fsg_led_probe(struct platform_device *pdev)
  failwan:
         led_classdev_unregister(&fsg_wlan_led);
  failwlan:
+        iounmap(latch_address);
+ failremap:
 
         return ret;
 }
 
 static int fsg_led_remove(struct platform_device *pdev)
 {
-        iounmap(latch_address);
-
         led_classdev_unregister(&fsg_wlan_led);
         led_classdev_unregister(&fsg_wan_led);
         led_classdev_unregister(&fsg_sata_led);
@@ -225,6 +223,8 @@ static int fsg_led_remove(struct platform_device *pdev)
         led_classdev_unregister(&fsg_sync_led);
         led_classdev_unregister(&fsg_ring_led);
 
+        iounmap(latch_address);
+
         return 0;
 }
 
diff --git a/drivers/leds/leds-pca955x.c b/drivers/leds/leds-pca955x.c
index 146c06972863..f508729123b5 100644
--- a/drivers/leds/leds-pca955x.c
+++ b/drivers/leds/leds-pca955x.c
@@ -248,11 +248,10 @@ static int __devinit pca955x_probe(struct i2c_client *client,
                                         const struct i2c_device_id *id)
 {
         struct pca955x_led *pca955x;
-        int i;
-        int err = -ENODEV;
         struct pca955x_chipdef *chip;
         struct i2c_adapter *adapter;
         struct led_platform_data *pdata;
+        int i, err;
 
         chip = &pca955x_chipdefs[id->driver_data];
         adapter = to_i2c_adapter(client->dev.parent);
@@ -282,43 +281,41 @@ static int __devinit pca955x_probe(struct i2c_client *client,
                 }
         }
 
+        pca955x = kzalloc(sizeof(*pca955x) * chip->bits, GFP_KERNEL);
+        if (!pca955x)
+                return -ENOMEM;
+
+        i2c_set_clientdata(client, pca955x);
+
         for (i = 0; i < chip->bits; i++) {
-                pca955x = kzalloc(sizeof(struct pca955x_led), GFP_KERNEL);
-                if (!pca955x) {
-                        err = -ENOMEM;
-                        goto exit;
-                }
+                pca955x[i].chipdef = chip;
+                pca955x[i].client = client;
+                pca955x[i].led_num = i;
 
-                pca955x->chipdef = chip;
-                pca955x->client = client;
-                pca955x->led_num = i;
                 /* Platform data can specify LED names and default triggers */
                 if (pdata) {
                         if (pdata->leds[i].name)
-                                snprintf(pca955x->name, 32, "pca955x:%s",
-                                                        pdata->leds[i].name);
+                                snprintf(pca955x[i].name,
+                                         sizeof(pca955x[i].name), "pca955x:%s",
+                                         pdata->leds[i].name);
                         if (pdata->leds[i].default_trigger)
-                                pca955x->led_cdev.default_trigger =
+                                pca955x[i].led_cdev.default_trigger =
                                         pdata->leds[i].default_trigger;
                 } else {
-                        snprintf(pca955x->name, 32, "pca955x:%d", i);
+                        snprintf(pca955x[i].name, sizeof(pca955x[i].name),
+                                 "pca955x:%d", i);
                 }
-                spin_lock_init(&pca955x->lock);
 
-                pca955x->led_cdev.name = pca955x->name;
-                pca955x->led_cdev.brightness_set =
-                                pca955x_led_set;
+                spin_lock_init(&pca955x[i].lock);
 
-                /*
-                 * Client data is a pointer to the _first_ pca955x_led
-                 * struct
-                 */
-                if (i == 0)
-                        i2c_set_clientdata(client, pca955x);
+                pca955x[i].led_cdev.name = pca955x[i].name;
+                pca955x[i].led_cdev.brightness_set = pca955x_led_set;
 
-                INIT_WORK(&(pca955x->work), pca955x_led_work);
+                INIT_WORK(&pca955x[i].work, pca955x_led_work);
 
-                led_classdev_register(&client->dev, &(pca955x->led_cdev));
+                err = led_classdev_register(&client->dev, &pca955x[i].led_cdev);
+                if (err < 0)
+                        goto exit;
         }
 
         /* Turn off LEDs */
@@ -336,23 +333,32 @@ static int __devinit pca955x_probe(struct i2c_client *client,
         pca955x_write_psc(client, 1, 0);
 
         return 0;
+
 exit:
+        while (i--) {
+                led_classdev_unregister(&pca955x[i].led_cdev);
+                cancel_work_sync(&pca955x[i].work);
+        }
+
+        kfree(pca955x);
+        i2c_set_clientdata(client, NULL);
+
         return err;
 }
 
 static int __devexit pca955x_remove(struct i2c_client *client)
 {
         struct pca955x_led *pca955x = i2c_get_clientdata(client);
-        int leds = pca955x->chipdef->bits;
         int i;
 
-        for (i = 0; i < leds; i++) {
-                led_classdev_unregister(&(pca955x->led_cdev));
-                cancel_work_sync(&(pca955x->work));
-                kfree(pca955x);
-                pca955x = pca955x + 1;
+        for (i = 0; i < pca955x->chipdef->bits; i++) {
+                led_classdev_unregister(&pca955x[i].led_cdev);
+                cancel_work_sync(&pca955x[i].work);
         }
 
+        kfree(pca955x);
+        i2c_set_clientdata(client, NULL);
+
         return 0;
 }