summaryrefslogtreecommitdiffstats
path: root/drivers/block/floppy.c
diff options
context:
space:
mode:
Diffstat (limited to 'drivers/block/floppy.c')
-rw-r--r--drivers/block/floppy.c93
1 files changed, 50 insertions, 43 deletions
diff --git a/drivers/block/floppy.c b/drivers/block/floppy.c
index 17c675c..2ddd64a 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,28 +4313,33 @@ 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;
+ destroy_workqueue(floppy_wq);
+ 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]);
}
return err;
}
@@ -4548,11 +4555,12 @@ static void __exit floppy_module_exit(void)
unregister_blkdev(FLOPPY_MAJOR, "fd");
platform_driver_unregister(&floppy_driver);
+ destroy_workqueue(floppy_wq);
+
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]);
@@ -4572,7 +4580,6 @@ static void __exit floppy_module_exit(void)
cancel_delayed_work_sync(&fd_timeout);
cancel_delayed_work_sync(&fd_timer);
- destroy_workqueue(floppy_wq);
if (atomic_read(&usage_count))
floppy_release_irq_and_dma();
OpenPOWER on IntegriCloud