Merge branch 'devel' of git://git.kernel.org/pub/scm/linux/kernel/git/ycmiao/pxa...

[linux-2.6-omap-h63xx.git] / drivers / block / floppy.c

diff --git a/drivers/block/floppy.c b/drivers/block/floppy.c
index cf29cc4e6ab73d2d95e6d4778f576aae9c98b327..83d8ed39433d058f70896057e52fc4d1e4dc99f3 100644 (file)
--- a/drivers/block/floppy.c
+++ b/drivers/block/floppy.c
@@ -558,6 +558,8 @@ static void process_fd_request(void);
 static void recalibrate_floppy(void);
 static void floppy_shutdown(unsigned long);
 
+static int floppy_request_regions(int);
+static void floppy_release_regions(int);
 static int floppy_grab_irq_and_dma(void);
 static void floppy_release_irq_and_dma(void);
 
@@ -4274,8 +4276,7 @@ static int __init floppy_init(void)
                FDCS->rawcmd = 2;
                if (user_reset_fdc(-1, FD_RESET_ALWAYS, 0)) {
                        /* free ioports reserved by floppy_grab_irq_and_dma() */
-                       release_region(FDCS->address + 2, 4);
-                       release_region(FDCS->address + 7, 1);
+                       floppy_release_regions(fdc);
                        FDCS->address = -1;
                        FDCS->version = FDC_NONE;
                        continue;
@@ -4284,8 +4285,7 @@ static int __init floppy_init(void)
                FDCS->version = get_fdc_version();
                if (FDCS->version == FDC_NONE) {
                        /* free ioports reserved by floppy_grab_irq_and_dma() */
-                       release_region(FDCS->address + 2, 4);
-                       release_region(FDCS->address + 7, 1);
+                       floppy_release_regions(fdc);
                        FDCS->address = -1;
                        continue;
                }
@@ -4358,6 +4358,47 @@ out_put_disk:
 
 static DEFINE_SPINLOCK(floppy_usage_lock);
 
+static const struct io_region {
+       int offset;
+       int size;
+} io_regions[] = {
+       { 2, 1 },
+       /* address + 3 is sometimes reserved by pnp bios for motherboard */
+       { 4, 2 },
+       /* address + 6 is reserved, and may be taken by IDE.
+        * Unfortunately, Adaptec doesn't know this :-(, */
+       { 7, 1 },
+};
+
+static void floppy_release_allocated_regions(int fdc, const struct io_region *p)
+{
+       while (p != io_regions) {
+               p--;
+               release_region(FDCS->address + p->offset, p->size);
+       }
+}
+
+#define ARRAY_END(X) (&((X)[ARRAY_SIZE(X)]))
+
+static int floppy_request_regions(int fdc)
+{
+       const struct io_region *p;
+
+       for (p = io_regions; p < ARRAY_END(io_regions); p++) {
+               if (!request_region(FDCS->address + p->offset, p->size, "floppy")) {
+                       DPRINT("Floppy io-port 0x%04lx in use\n", FDCS->address + p->offset);
+                       floppy_release_allocated_regions(fdc, p);
+                       return -EBUSY;
+               }
+       }
+       return 0;
+}
+
+static void floppy_release_regions(int fdc)
+{
+       floppy_release_allocated_regions(fdc, ARRAY_END(io_regions));
+}
+
 static int floppy_grab_irq_and_dma(void)
 {
        unsigned long flags;
@@ -4399,18 +4440,8 @@ static int floppy_grab_irq_and_dma(void)
 
        for (fdc = 0; fdc < N_FDC; fdc++) {
                if (FDCS->address != -1) {
-                       if (!request_region(FDCS->address + 2, 4, "floppy")) {
-                               DPRINT("Floppy io-port 0x%04lx in use\n",
-                                      FDCS->address + 2);
-                               goto cleanup1;
-                       }
-                       if (!request_region(FDCS->address + 7, 1, "floppy DIR")) {
-                               DPRINT("Floppy io-port 0x%04lx in use\n",
-                                      FDCS->address + 7);
-                               goto cleanup2;
-                       }
-                       /* address + 6 is reserved, and may be taken by IDE.
-                        * Unfortunately, Adaptec doesn't know this :-(, */
+                       if (floppy_request_regions(fdc))
+                               goto cleanup;
                }
        }
        for (fdc = 0; fdc < N_FDC; fdc++) {
@@ -4432,15 +4463,11 @@ static int floppy_grab_irq_and_dma(void)
        fdc = 0;
        irqdma_allocated = 1;
        return 0;
-cleanup2:
-       release_region(FDCS->address + 2, 4);
-cleanup1:
+cleanup:
        fd_free_irq();
        fd_free_dma();
-       while (--fdc >= 0) {
-               release_region(FDCS->address + 2, 4);
-               release_region(FDCS->address + 7, 1);
-       }
+       while (--fdc >= 0)
+               floppy_release_regions(fdc);
        spin_lock_irqsave(&floppy_usage_lock, flags);
        usage_count--;
        spin_unlock_irqrestore(&floppy_usage_lock, flags);
@@ -4501,10 +4528,8 @@ static void floppy_release_irq_and_dma(void)
 #endif
        old_fdc = fdc;
        for (fdc = 0; fdc < N_FDC; fdc++)
-               if (FDCS->address != -1) {
-                       release_region(FDCS->address + 2, 4);
-                       release_region(FDCS->address + 7, 1);
-               }
+               if (FDCS->address != -1)
+                       floppy_release_regions(fdc);
        fdc = old_fdc;
 }