|
@@ -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]);
|