1 files changed, 48 insertions, 42 deletions

diff --git a/drivers/block/floppy.c b/drivers/block/floppy.c
index 17c675c52295..1c49d7173966 100644
--- a/drivers/block/floppy.c
+++ b/drivers/block/floppy.c
@@ -4109,12 +4109,19 @@ static struct platform_driver floppy_driver = {
 
 static struct platform_device floppy_device[N_DRIVE];
 
+static bool floppy_available(int drive)
+{
+        if (!(allowed_drive_mask & (1 << drive)))
+                return false;
+        if (fdc_state[FDC(drive)].version == FDC_NONE)
+                return false;
+        return true;
+}
+
 static struct kobject *floppy_find(dev_t dev, int *part, void *data)
 {
         int drive = (*part & 3) | ((*part & 0x80) >> 5);
-        if (drive >= N_DRIVE ||
-            !(allowed_drive_mask & (1 << drive)) ||
-            fdc_state[FDC(drive)].version == FDC_NONE)
+        if (drive >= N_DRIVE || !floppy_available(drive))
                 return NULL;
         if (((*part >> 2) & 0x1f) >= ARRAY_SIZE(floppy_type))
                 return NULL;
@@ -4124,8 +4131,7 @@ static struct kobject *floppy_find(dev_t dev, int *part, void *data)
 
 static int __init do_floppy_init(void)
 {
-        int i, unit, drive;
-        int err, dr;
+        int i, unit, drive, err;
 
         set_debugt();
         interruptjiffies = resultjiffies = jiffies;
@@ -4137,34 +4143,32 @@ static int __init do_floppy_init(void)
 
         raw_cmd = NULL;
 
-        for (dr = 0; dr < N_DRIVE; dr++) {
-                disks[dr] = alloc_disk(1);
-                if (!disks[dr]) {
-                        err = -ENOMEM;
-                        goto out_put_disk;
-                }
+        floppy_wq = alloc_ordered_workqueue("floppy", 0);
+        if (!floppy_wq)
+                return -ENOMEM;
 
-                floppy_wq = alloc_ordered_workqueue("floppy", 0);
-                if (!floppy_wq) {
+        for (drive = 0; drive < N_DRIVE; drive++) {
+                disks[drive] = alloc_disk(1);
+                if (!disks[drive]) {
                         err = -ENOMEM;
                         goto out_put_disk;
                 }
 
-                disks[dr]->queue = blk_init_queue(do_fd_request, &floppy_lock);
-                if (!disks[dr]->queue) {
+                disks[drive]->queue = blk_init_queue(do_fd_request, &floppy_lock);
+                if (!disks[drive]->queue) {
                         err = -ENOMEM;
-                        goto out_destroy_workq;
+                        goto out_put_disk;
                 }
 
-                blk_queue_max_hw_sectors(disks[dr]->queue, 64);
-                disks[dr]->major = FLOPPY_MAJOR;
-                disks[dr]->first_minor = TOMINOR(dr);
-                disks[dr]->fops = &floppy_fops;
-                sprintf(disks[dr]->disk_name, "fd%d", dr);
+                blk_queue_max_hw_sectors(disks[drive]->queue, 64);
+                disks[drive]->major = FLOPPY_MAJOR;
+                disks[drive]->first_minor = TOMINOR(drive);
+                disks[drive]->fops = &floppy_fops;
+                sprintf(disks[drive]->disk_name, "fd%d", drive);
 
-                init_timer(&motor_off_timer[dr]);
-                motor_off_timer[dr].data = dr;
-                motor_off_timer[dr].function = motor_off_callback;
+                init_timer(&motor_off_timer[drive]);
+                motor_off_timer[drive].data = drive;
+                motor_off_timer[drive].function = motor_off_callback;
         }
 
         err = register_blkdev(FLOPPY_MAJOR, "fd");
@@ -4282,9 +4286,7 @@ static int __init do_floppy_init(void)
         }
 
         for (drive = 0; drive < N_DRIVE; drive++) {
-                if (!(allowed_drive_mask & (1 << drive)))
-                        continue;
-                if (fdc_state[FDC(drive)].version == FDC_NONE)
+                if (!floppy_available(drive))
                         continue;
 
                 floppy_device[drive].name = floppy_device_name;
@@ -4293,7 +4295,7 @@ static int __init do_floppy_init(void)
 
                 err = platform_device_register(&floppy_device[drive]);
                 if (err)
-                        goto out_release_dma;
+                        goto out_remove_drives;
 
                 err = device_create_file(&floppy_device[drive].dev,
                                          &dev_attr_cmos);
@@ -4311,29 +4313,34 @@ static int __init do_floppy_init(void)
 
 out_unreg_platform_dev:
         platform_device_unregister(&floppy_device[drive]);
+out_remove_drives:
+        while (drive--) {
+                if (floppy_available(drive)) {
+                        del_gendisk(disks[drive]);
+                        device_remove_file(&floppy_device[drive].dev, &dev_attr_cmos);
+                        platform_device_unregister(&floppy_device[drive]);
+                }
+        }
 out_release_dma:
         if (atomic_read(&usage_count))
                 floppy_release_irq_and_dma();
 out_unreg_region:
         blk_unregister_region(MKDEV(FLOPPY_MAJOR, 0), 256);
         platform_driver_unregister(&floppy_driver);
-out_destroy_workq:
-        destroy_workqueue(floppy_wq);
 out_unreg_blkdev:
         unregister_blkdev(FLOPPY_MAJOR, "fd");
 out_put_disk:
-        while (dr--) {
-                del_timer_sync(&motor_off_timer[dr]);
-                if (disks[dr]->queue) {
-                        blk_cleanup_queue(disks[dr]->queue);
-                        /*
-                         * put_disk() is not paired with add_disk() and
-                         * will put queue reference one extra time. fix it.
-                         */
-                        disks[dr]->queue = NULL;
+        for (drive = 0; drive < N_DRIVE; drive++) {
+                if (!disks[drive])
+                        break;
+                if (disks[drive]->queue) {
+                        del_timer_sync(&motor_off_timer[drive]);
+                        blk_cleanup_queue(disks[drive]->queue);
+                        disks[drive]->queue = NULL;
                 }
-                put_disk(disks[dr]);
+                put_disk(disks[drive]);
         }
+        destroy_workqueue(floppy_wq);
         return err;
 }
 
@@ -4551,8 +4558,7 @@ static void __exit floppy_module_exit(void)
         for (drive = 0; drive < N_DRIVE; drive++) {
                 del_timer_sync(&motor_off_timer[drive]);
 
-                if ((allowed_drive_mask & (1 << drive)) &&
-                    fdc_state[FDC(drive)].version != FDC_NONE) {
+                if (floppy_available(drive)) {
                         del_gendisk(disks[drive]);
                         device_remove_file(&floppy_device[drive].dev, &dev_attr_cmos);
                         platform_device_unregister(&floppy_device[drive]);