1 files changed, 53 insertions, 29 deletions

diff --git a/drivers/block/floppy.c b/drivers/block/floppy.c
index cf29cc4e6ab7..c2c95e614506 100644
--- 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);
 
@@ -4133,10 +4135,9 @@ static int have_no_fdc = -ENODEV;
 static ssize_t floppy_cmos_show(struct device *dev,
 				struct device_attribute *attr, char *buf)
 {
-	struct platform_device *p;
+	struct platform_device *p = to_platform_device(dev);
 	int drive;
 
-	p = container_of(dev, struct platform_device,dev);
 	drive = p->id;
 	return sprintf(buf, "%X\n", UDP->cmos);
 }
@@ -4274,8 +4275,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 +4284,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 +4357,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 +4439,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 +4462,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 +4527,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;
 }