xref: /openbmc/linux/drivers/block/swim3.c (revision d21fed50)
1 // SPDX-License-Identifier: GPL-2.0-or-later
2 /*
3  * Driver for the SWIM3 (Super Woz Integrated Machine 3)
4  * floppy controller found on Power Macintoshes.
5  *
6  * Copyright (C) 1996 Paul Mackerras.
7  */
8 
9 /*
10  * TODO:
11  * handle 2 drives
12  * handle GCR disks
13  */
14 
15 #undef DEBUG
16 
17 #include <linux/stddef.h>
18 #include <linux/kernel.h>
19 #include <linux/sched/signal.h>
20 #include <linux/timer.h>
21 #include <linux/delay.h>
22 #include <linux/fd.h>
23 #include <linux/ioctl.h>
24 #include <linux/blk-mq.h>
25 #include <linux/interrupt.h>
26 #include <linux/mutex.h>
27 #include <linux/module.h>
28 #include <linux/spinlock.h>
29 #include <linux/wait.h>
30 #include <linux/major.h>
31 #include <asm/io.h>
32 #include <asm/dbdma.h>
33 #include <asm/prom.h>
34 #include <linux/uaccess.h>
35 #include <asm/mediabay.h>
36 #include <asm/machdep.h>
37 #include <asm/pmac_feature.h>
38 
39 #define MAX_FLOPPIES	2
40 
41 static DEFINE_MUTEX(swim3_mutex);
42 static struct gendisk *disks[MAX_FLOPPIES];
43 
44 enum swim_state {
45 	idle,
46 	locating,
47 	seeking,
48 	settling,
49 	do_transfer,
50 	jogging,
51 	available,
52 	revalidating,
53 	ejecting
54 };
55 
56 #define REG(x)	unsigned char x; char x ## _pad[15];
57 
58 /*
59  * The names for these registers mostly represent speculation on my part.
60  * It will be interesting to see how close they are to the names Apple uses.
61  */
62 struct swim3 {
63 	REG(data);
64 	REG(timer);		/* counts down at 1MHz */
65 	REG(error);
66 	REG(mode);
67 	REG(select);		/* controls CA0, CA1, CA2 and LSTRB signals */
68 	REG(setup);
69 	REG(control);		/* writing bits clears them */
70 	REG(status);		/* writing bits sets them in control */
71 	REG(intr);
72 	REG(nseek);		/* # tracks to seek */
73 	REG(ctrack);		/* current track number */
74 	REG(csect);		/* current sector number */
75 	REG(gap3);		/* size of gap 3 in track format */
76 	REG(sector);		/* sector # to read or write */
77 	REG(nsect);		/* # sectors to read or write */
78 	REG(intr_enable);
79 };
80 
81 #define control_bic	control
82 #define control_bis	status
83 
84 /* Bits in select register */
85 #define CA_MASK		7
86 #define LSTRB		8
87 
88 /* Bits in control register */
89 #define DO_SEEK		0x80
90 #define FORMAT		0x40
91 #define SELECT		0x20
92 #define WRITE_SECTORS	0x10
93 #define DO_ACTION	0x08
94 #define DRIVE2_ENABLE	0x04
95 #define DRIVE_ENABLE	0x02
96 #define INTR_ENABLE	0x01
97 
98 /* Bits in status register */
99 #define FIFO_1BYTE	0x80
100 #define FIFO_2BYTE	0x40
101 #define ERROR		0x20
102 #define DATA		0x08
103 #define RDDATA		0x04
104 #define INTR_PENDING	0x02
105 #define MARK_BYTE	0x01
106 
107 /* Bits in intr and intr_enable registers */
108 #define ERROR_INTR	0x20
109 #define DATA_CHANGED	0x10
110 #define TRANSFER_DONE	0x08
111 #define SEEN_SECTOR	0x04
112 #define SEEK_DONE	0x02
113 #define TIMER_DONE	0x01
114 
115 /* Bits in error register */
116 #define ERR_DATA_CRC	0x80
117 #define ERR_ADDR_CRC	0x40
118 #define ERR_OVERRUN	0x04
119 #define ERR_UNDERRUN	0x01
120 
121 /* Bits in setup register */
122 #define S_SW_RESET	0x80
123 #define S_GCR_WRITE	0x40
124 #define S_IBM_DRIVE	0x20
125 #define S_TEST_MODE	0x10
126 #define S_FCLK_DIV2	0x08
127 #define S_GCR		0x04
128 #define S_COPY_PROT	0x02
129 #define S_INV_WDATA	0x01
130 
131 /* Select values for swim3_action */
132 #define SEEK_POSITIVE	0
133 #define SEEK_NEGATIVE	4
134 #define STEP		1
135 #define MOTOR_ON	2
136 #define MOTOR_OFF	6
137 #define INDEX		3
138 #define EJECT		7
139 #define SETMFM		9
140 #define SETGCR		13
141 
142 /* Select values for swim3_select and swim3_readbit */
143 #define STEP_DIR	0
144 #define STEPPING	1
145 #define MOTOR_ON	2
146 #define RELAX		3	/* also eject in progress */
147 #define READ_DATA_0	4
148 #define ONEMEG_DRIVE	5
149 #define SINGLE_SIDED	6	/* drive or diskette is 4MB type? */
150 #define DRIVE_PRESENT	7
151 #define DISK_IN		8
152 #define WRITE_PROT	9
153 #define TRACK_ZERO	10
154 #define TACHO		11
155 #define READ_DATA_1	12
156 #define GCR_MODE	13
157 #define SEEK_COMPLETE	14
158 #define TWOMEG_MEDIA	15
159 
160 /* Definitions of values used in writing and formatting */
161 #define DATA_ESCAPE	0x99
162 #define GCR_SYNC_EXC	0x3f
163 #define GCR_SYNC_CONV	0x80
164 #define GCR_FIRST_MARK	0xd5
165 #define GCR_SECOND_MARK	0xaa
166 #define GCR_ADDR_MARK	"\xd5\xaa\x00"
167 #define GCR_DATA_MARK	"\xd5\xaa\x0b"
168 #define GCR_SLIP_BYTE	"\x27\xaa"
169 #define GCR_SELF_SYNC	"\x3f\xbf\x1e\x34\x3c\x3f"
170 
171 #define DATA_99		"\x99\x99"
172 #define MFM_ADDR_MARK	"\x99\xa1\x99\xa1\x99\xa1\x99\xfe"
173 #define MFM_INDEX_MARK	"\x99\xc2\x99\xc2\x99\xc2\x99\xfc"
174 #define MFM_GAP_LEN	12
175 
176 struct floppy_state {
177 	enum swim_state	state;
178 	struct swim3 __iomem *swim3;	/* hardware registers */
179 	struct dbdma_regs __iomem *dma;	/* DMA controller registers */
180 	int	swim3_intr;	/* interrupt number for SWIM3 */
181 	int	dma_intr;	/* interrupt number for DMA channel */
182 	int	cur_cyl;	/* cylinder head is on, or -1 */
183 	int	cur_sector;	/* last sector we saw go past */
184 	int	req_cyl;	/* the cylinder for the current r/w request */
185 	int	head;		/* head number ditto */
186 	int	req_sector;	/* sector number ditto */
187 	int	scount;		/* # sectors we're transferring at present */
188 	int	retries;
189 	int	settle_time;
190 	int	secpercyl;	/* disk geometry information */
191 	int	secpertrack;
192 	int	total_secs;
193 	int	write_prot;	/* 1 if write-protected, 0 if not, -1 dunno */
194 	struct dbdma_cmd *dma_cmd;
195 	int	ref_count;
196 	int	expect_cyl;
197 	struct timer_list timeout;
198 	int	timeout_pending;
199 	int	ejected;
200 	wait_queue_head_t wait;
201 	int	wanted;
202 	struct macio_dev *mdev;
203 	char	dbdma_cmd_space[5 * sizeof(struct dbdma_cmd)];
204 	int	index;
205 	struct request *cur_req;
206 	struct blk_mq_tag_set tag_set;
207 };
208 
209 #define swim3_err(fmt, arg...)	dev_err(&fs->mdev->ofdev.dev, "[fd%d] " fmt, fs->index, arg)
210 #define swim3_warn(fmt, arg...)	dev_warn(&fs->mdev->ofdev.dev, "[fd%d] " fmt, fs->index, arg)
211 #define swim3_info(fmt, arg...)	dev_info(&fs->mdev->ofdev.dev, "[fd%d] " fmt, fs->index, arg)
212 
213 #ifdef DEBUG
214 #define swim3_dbg(fmt, arg...)	dev_dbg(&fs->mdev->ofdev.dev, "[fd%d] " fmt, fs->index, arg)
215 #else
216 #define swim3_dbg(fmt, arg...)	do { } while(0)
217 #endif
218 
219 static struct floppy_state floppy_states[MAX_FLOPPIES];
220 static int floppy_count = 0;
221 static DEFINE_SPINLOCK(swim3_lock);
222 
223 static unsigned short write_preamble[] = {
224 	0x4e4e, 0x4e4e, 0x4e4e, 0x4e4e, 0x4e4e,	/* gap field */
225 	0, 0, 0, 0, 0, 0,			/* sync field */
226 	0x99a1, 0x99a1, 0x99a1, 0x99fb,		/* data address mark */
227 	0x990f					/* no escape for 512 bytes */
228 };
229 
230 static unsigned short write_postamble[] = {
231 	0x9904,					/* insert CRC */
232 	0x4e4e, 0x4e4e,
233 	0x9908,					/* stop writing */
234 	0, 0, 0, 0, 0, 0
235 };
236 
237 static void seek_track(struct floppy_state *fs, int n);
238 static void act(struct floppy_state *fs);
239 static void scan_timeout(struct timer_list *t);
240 static void seek_timeout(struct timer_list *t);
241 static void settle_timeout(struct timer_list *t);
242 static void xfer_timeout(struct timer_list *t);
243 static irqreturn_t swim3_interrupt(int irq, void *dev_id);
244 /*static void fd_dma_interrupt(int irq, void *dev_id);*/
245 static int grab_drive(struct floppy_state *fs, enum swim_state state,
246 		      int interruptible);
247 static void release_drive(struct floppy_state *fs);
248 static int fd_eject(struct floppy_state *fs);
249 static int floppy_ioctl(struct block_device *bdev, blk_mode_t mode,
250 			unsigned int cmd, unsigned long param);
251 static int floppy_open(struct gendisk *disk, blk_mode_t mode);
252 static unsigned int floppy_check_events(struct gendisk *disk,
253 					unsigned int clearing);
254 static int floppy_revalidate(struct gendisk *disk);
255 
swim3_end_request(struct floppy_state * fs,blk_status_t err,unsigned int nr_bytes)256 static bool swim3_end_request(struct floppy_state *fs, blk_status_t err, unsigned int nr_bytes)
257 {
258 	struct request *req = fs->cur_req;
259 
260 	swim3_dbg("  end request, err=%d nr_bytes=%d, cur_req=%p\n",
261 		  err, nr_bytes, req);
262 
263 	if (err)
264 		nr_bytes = blk_rq_cur_bytes(req);
265 	if (blk_update_request(req, err, nr_bytes))
266 		return true;
267 	__blk_mq_end_request(req, err);
268 	fs->cur_req = NULL;
269 	return false;
270 }
271 
swim3_select(struct floppy_state * fs,int sel)272 static void swim3_select(struct floppy_state *fs, int sel)
273 {
274 	struct swim3 __iomem *sw = fs->swim3;
275 
276 	out_8(&sw->select, RELAX);
277 	if (sel & 8)
278 		out_8(&sw->control_bis, SELECT);
279 	else
280 		out_8(&sw->control_bic, SELECT);
281 	out_8(&sw->select, sel & CA_MASK);
282 }
283 
swim3_action(struct floppy_state * fs,int action)284 static void swim3_action(struct floppy_state *fs, int action)
285 {
286 	struct swim3 __iomem *sw = fs->swim3;
287 
288 	swim3_select(fs, action);
289 	udelay(1);
290 	out_8(&sw->select, sw->select | LSTRB);
291 	udelay(2);
292 	out_8(&sw->select, sw->select & ~LSTRB);
293 	udelay(1);
294 }
295 
swim3_readbit(struct floppy_state * fs,int bit)296 static int swim3_readbit(struct floppy_state *fs, int bit)
297 {
298 	struct swim3 __iomem *sw = fs->swim3;
299 	int stat;
300 
301 	swim3_select(fs, bit);
302 	udelay(1);
303 	stat = in_8(&sw->status);
304 	return (stat & DATA) == 0;
305 }
306 
swim3_queue_rq(struct blk_mq_hw_ctx * hctx,const struct blk_mq_queue_data * bd)307 static blk_status_t swim3_queue_rq(struct blk_mq_hw_ctx *hctx,
308 				   const struct blk_mq_queue_data *bd)
309 {
310 	struct floppy_state *fs = hctx->queue->queuedata;
311 	struct request *req = bd->rq;
312 	unsigned long x;
313 
314 	spin_lock_irq(&swim3_lock);
315 	if (fs->cur_req || fs->state != idle) {
316 		spin_unlock_irq(&swim3_lock);
317 		return BLK_STS_DEV_RESOURCE;
318 	}
319 	blk_mq_start_request(req);
320 	fs->cur_req = req;
321 	if (fs->mdev->media_bay &&
322 	    check_media_bay(fs->mdev->media_bay) != MB_FD) {
323 		swim3_dbg("%s", "  media bay absent, dropping req\n");
324 		swim3_end_request(fs, BLK_STS_IOERR, 0);
325 		goto out;
326 	}
327 	if (fs->ejected) {
328 		swim3_dbg("%s", "  disk ejected\n");
329 		swim3_end_request(fs, BLK_STS_IOERR, 0);
330 		goto out;
331 	}
332 	if (rq_data_dir(req) == WRITE) {
333 		if (fs->write_prot < 0)
334 			fs->write_prot = swim3_readbit(fs, WRITE_PROT);
335 		if (fs->write_prot) {
336 			swim3_dbg("%s", "  try to write, disk write protected\n");
337 			swim3_end_request(fs, BLK_STS_IOERR, 0);
338 			goto out;
339 		}
340 	}
341 
342 	/*
343 	 * Do not remove the cast. blk_rq_pos(req) is now a sector_t and can be
344 	 * 64 bits, but it will never go past 32 bits for this driver anyway, so
345 	 * we can safely cast it down and not have to do a 64/32 division
346 	 */
347 	fs->req_cyl = ((long)blk_rq_pos(req)) / fs->secpercyl;
348 	x = ((long)blk_rq_pos(req)) % fs->secpercyl;
349 	fs->head = x / fs->secpertrack;
350 	fs->req_sector = x % fs->secpertrack + 1;
351 	fs->state = do_transfer;
352 	fs->retries = 0;
353 
354 	act(fs);
355 
356 out:
357 	spin_unlock_irq(&swim3_lock);
358 	return BLK_STS_OK;
359 }
360 
set_timeout(struct floppy_state * fs,int nticks,void (* proc)(struct timer_list * t))361 static void set_timeout(struct floppy_state *fs, int nticks,
362 			void (*proc)(struct timer_list *t))
363 {
364 	if (fs->timeout_pending)
365 		del_timer(&fs->timeout);
366 	fs->timeout.expires = jiffies + nticks;
367 	fs->timeout.function = proc;
368 	add_timer(&fs->timeout);
369 	fs->timeout_pending = 1;
370 }
371 
scan_track(struct floppy_state * fs)372 static inline void scan_track(struct floppy_state *fs)
373 {
374 	struct swim3 __iomem *sw = fs->swim3;
375 
376 	swim3_select(fs, READ_DATA_0);
377 	in_8(&sw->intr);		/* clear SEEN_SECTOR bit */
378 	in_8(&sw->error);
379 	out_8(&sw->intr_enable, SEEN_SECTOR);
380 	out_8(&sw->control_bis, DO_ACTION);
381 	/* enable intr when track found */
382 	set_timeout(fs, HZ, scan_timeout);	/* enable timeout */
383 }
384 
seek_track(struct floppy_state * fs,int n)385 static inline void seek_track(struct floppy_state *fs, int n)
386 {
387 	struct swim3 __iomem *sw = fs->swim3;
388 
389 	if (n >= 0) {
390 		swim3_action(fs, SEEK_POSITIVE);
391 		sw->nseek = n;
392 	} else {
393 		swim3_action(fs, SEEK_NEGATIVE);
394 		sw->nseek = -n;
395 	}
396 	fs->expect_cyl = (fs->cur_cyl >= 0)? fs->cur_cyl + n: -1;
397 	swim3_select(fs, STEP);
398 	in_8(&sw->error);
399 	/* enable intr when seek finished */
400 	out_8(&sw->intr_enable, SEEK_DONE);
401 	out_8(&sw->control_bis, DO_SEEK);
402 	set_timeout(fs, 3*HZ, seek_timeout);	/* enable timeout */
403 	fs->settle_time = 0;
404 }
405 
406 /*
407  * XXX: this is a horrible hack, but at least allows ppc32 to get
408  * out of defining virt_to_bus, and this driver out of using the
409  * deprecated block layer bounce buffering for highmem addresses
410  * for no good reason.
411  */
swim3_phys_to_bus(phys_addr_t paddr)412 static unsigned long swim3_phys_to_bus(phys_addr_t paddr)
413 {
414 	return paddr + PCI_DRAM_OFFSET;
415 }
416 
swim3_bio_phys(struct bio * bio)417 static phys_addr_t swim3_bio_phys(struct bio *bio)
418 {
419 	return page_to_phys(bio_page(bio)) + bio_offset(bio);
420 }
421 
init_dma(struct dbdma_cmd * cp,int cmd,phys_addr_t paddr,int count)422 static inline void init_dma(struct dbdma_cmd *cp, int cmd,
423 			    phys_addr_t paddr, int count)
424 {
425 	cp->req_count = cpu_to_le16(count);
426 	cp->command = cpu_to_le16(cmd);
427 	cp->phy_addr = cpu_to_le32(swim3_phys_to_bus(paddr));
428 	cp->xfer_status = 0;
429 }
430 
setup_transfer(struct floppy_state * fs)431 static inline void setup_transfer(struct floppy_state *fs)
432 {
433 	int n;
434 	struct swim3 __iomem *sw = fs->swim3;
435 	struct dbdma_cmd *cp = fs->dma_cmd;
436 	struct dbdma_regs __iomem *dr = fs->dma;
437 	struct request *req = fs->cur_req;
438 
439 	if (blk_rq_cur_sectors(req) <= 0) {
440 		swim3_warn("%s", "Transfer 0 sectors ?\n");
441 		return;
442 	}
443 	if (rq_data_dir(req) == WRITE)
444 		n = 1;
445 	else {
446 		n = fs->secpertrack - fs->req_sector + 1;
447 		if (n > blk_rq_cur_sectors(req))
448 			n = blk_rq_cur_sectors(req);
449 	}
450 
451 	swim3_dbg("  setup xfer at sect %d (of %d) head %d for %d\n",
452 		  fs->req_sector, fs->secpertrack, fs->head, n);
453 
454 	fs->scount = n;
455 	swim3_select(fs, fs->head? READ_DATA_1: READ_DATA_0);
456 	out_8(&sw->sector, fs->req_sector);
457 	out_8(&sw->nsect, n);
458 	out_8(&sw->gap3, 0);
459 	out_le32(&dr->cmdptr, swim3_phys_to_bus(virt_to_phys(cp)));
460 	if (rq_data_dir(req) == WRITE) {
461 		/* Set up 3 dma commands: write preamble, data, postamble */
462 		init_dma(cp, OUTPUT_MORE, virt_to_phys(write_preamble),
463 			 sizeof(write_preamble));
464 		++cp;
465 		init_dma(cp, OUTPUT_MORE, swim3_bio_phys(req->bio), 512);
466 		++cp;
467 		init_dma(cp, OUTPUT_LAST, virt_to_phys(write_postamble),
468 			sizeof(write_postamble));
469 	} else {
470 		init_dma(cp, INPUT_LAST, swim3_bio_phys(req->bio), n * 512);
471 	}
472 	++cp;
473 	out_le16(&cp->command, DBDMA_STOP);
474 	out_8(&sw->control_bic, DO_ACTION | WRITE_SECTORS);
475 	in_8(&sw->error);
476 	out_8(&sw->control_bic, DO_ACTION | WRITE_SECTORS);
477 	if (rq_data_dir(req) == WRITE)
478 		out_8(&sw->control_bis, WRITE_SECTORS);
479 	in_8(&sw->intr);
480 	out_le32(&dr->control, (RUN << 16) | RUN);
481 	/* enable intr when transfer complete */
482 	out_8(&sw->intr_enable, TRANSFER_DONE);
483 	out_8(&sw->control_bis, DO_ACTION);
484 	set_timeout(fs, 2*HZ, xfer_timeout);	/* enable timeout */
485 }
486 
act(struct floppy_state * fs)487 static void act(struct floppy_state *fs)
488 {
489 	for (;;) {
490 		swim3_dbg("  act loop, state=%d, req_cyl=%d, cur_cyl=%d\n",
491 			  fs->state, fs->req_cyl, fs->cur_cyl);
492 
493 		switch (fs->state) {
494 		case idle:
495 			return;		/* XXX shouldn't get here */
496 
497 		case locating:
498 			if (swim3_readbit(fs, TRACK_ZERO)) {
499 				swim3_dbg("%s", "    locate track 0\n");
500 				fs->cur_cyl = 0;
501 				if (fs->req_cyl == 0)
502 					fs->state = do_transfer;
503 				else
504 					fs->state = seeking;
505 				break;
506 			}
507 			scan_track(fs);
508 			return;
509 
510 		case seeking:
511 			if (fs->cur_cyl < 0) {
512 				fs->expect_cyl = -1;
513 				fs->state = locating;
514 				break;
515 			}
516 			if (fs->req_cyl == fs->cur_cyl) {
517 				swim3_warn("%s", "Whoops, seeking 0\n");
518 				fs->state = do_transfer;
519 				break;
520 			}
521 			seek_track(fs, fs->req_cyl - fs->cur_cyl);
522 			return;
523 
524 		case settling:
525 			/* check for SEEK_COMPLETE after 30ms */
526 			fs->settle_time = (HZ + 32) / 33;
527 			set_timeout(fs, fs->settle_time, settle_timeout);
528 			return;
529 
530 		case do_transfer:
531 			if (fs->cur_cyl != fs->req_cyl) {
532 				if (fs->retries > 5) {
533 					swim3_err("Wrong cylinder in transfer, want: %d got %d\n",
534 						  fs->req_cyl, fs->cur_cyl);
535 					swim3_end_request(fs, BLK_STS_IOERR, 0);
536 					fs->state = idle;
537 					return;
538 				}
539 				fs->state = seeking;
540 				break;
541 			}
542 			setup_transfer(fs);
543 			return;
544 
545 		case jogging:
546 			seek_track(fs, -5);
547 			return;
548 
549 		default:
550 			swim3_err("Unknown state %d\n", fs->state);
551 			return;
552 		}
553 	}
554 }
555 
scan_timeout(struct timer_list * t)556 static void scan_timeout(struct timer_list *t)
557 {
558 	struct floppy_state *fs = from_timer(fs, t, timeout);
559 	struct swim3 __iomem *sw = fs->swim3;
560 	unsigned long flags;
561 
562 	swim3_dbg("* scan timeout, state=%d\n", fs->state);
563 
564 	spin_lock_irqsave(&swim3_lock, flags);
565 	fs->timeout_pending = 0;
566 	out_8(&sw->control_bic, DO_ACTION | WRITE_SECTORS);
567 	out_8(&sw->select, RELAX);
568 	out_8(&sw->intr_enable, 0);
569 	fs->cur_cyl = -1;
570 	if (fs->retries > 5) {
571 		swim3_end_request(fs, BLK_STS_IOERR, 0);
572 		fs->state = idle;
573 	} else {
574 		fs->state = jogging;
575 		act(fs);
576 	}
577 	spin_unlock_irqrestore(&swim3_lock, flags);
578 }
579 
seek_timeout(struct timer_list * t)580 static void seek_timeout(struct timer_list *t)
581 {
582 	struct floppy_state *fs = from_timer(fs, t, timeout);
583 	struct swim3 __iomem *sw = fs->swim3;
584 	unsigned long flags;
585 
586 	swim3_dbg("* seek timeout, state=%d\n", fs->state);
587 
588 	spin_lock_irqsave(&swim3_lock, flags);
589 	fs->timeout_pending = 0;
590 	out_8(&sw->control_bic, DO_SEEK);
591 	out_8(&sw->select, RELAX);
592 	out_8(&sw->intr_enable, 0);
593 	swim3_err("%s", "Seek timeout\n");
594 	swim3_end_request(fs, BLK_STS_IOERR, 0);
595 	fs->state = idle;
596 	spin_unlock_irqrestore(&swim3_lock, flags);
597 }
598 
settle_timeout(struct timer_list * t)599 static void settle_timeout(struct timer_list *t)
600 {
601 	struct floppy_state *fs = from_timer(fs, t, timeout);
602 	struct swim3 __iomem *sw = fs->swim3;
603 	unsigned long flags;
604 
605 	swim3_dbg("* settle timeout, state=%d\n", fs->state);
606 
607 	spin_lock_irqsave(&swim3_lock, flags);
608 	fs->timeout_pending = 0;
609 	if (swim3_readbit(fs, SEEK_COMPLETE)) {
610 		out_8(&sw->select, RELAX);
611 		fs->state = locating;
612 		act(fs);
613 		goto unlock;
614 	}
615 	out_8(&sw->select, RELAX);
616 	if (fs->settle_time < 2*HZ) {
617 		++fs->settle_time;
618 		set_timeout(fs, 1, settle_timeout);
619 		goto unlock;
620 	}
621 	swim3_err("%s", "Seek settle timeout\n");
622 	swim3_end_request(fs, BLK_STS_IOERR, 0);
623 	fs->state = idle;
624  unlock:
625 	spin_unlock_irqrestore(&swim3_lock, flags);
626 }
627 
xfer_timeout(struct timer_list * t)628 static void xfer_timeout(struct timer_list *t)
629 {
630 	struct floppy_state *fs = from_timer(fs, t, timeout);
631 	struct swim3 __iomem *sw = fs->swim3;
632 	struct dbdma_regs __iomem *dr = fs->dma;
633 	unsigned long flags;
634 	int n;
635 
636 	swim3_dbg("* xfer timeout, state=%d\n", fs->state);
637 
638 	spin_lock_irqsave(&swim3_lock, flags);
639 	fs->timeout_pending = 0;
640 	out_le32(&dr->control, RUN << 16);
641 	/* We must wait a bit for dbdma to stop */
642 	for (n = 0; (in_le32(&dr->status) & ACTIVE) && n < 1000; n++)
643 		udelay(1);
644 	out_8(&sw->intr_enable, 0);
645 	out_8(&sw->control_bic, WRITE_SECTORS | DO_ACTION);
646 	out_8(&sw->select, RELAX);
647 	swim3_err("Timeout %sing sector %ld\n",
648 	       (rq_data_dir(fs->cur_req)==WRITE? "writ": "read"),
649 	       (long)blk_rq_pos(fs->cur_req));
650 	swim3_end_request(fs, BLK_STS_IOERR, 0);
651 	fs->state = idle;
652 	spin_unlock_irqrestore(&swim3_lock, flags);
653 }
654 
swim3_interrupt(int irq,void * dev_id)655 static irqreturn_t swim3_interrupt(int irq, void *dev_id)
656 {
657 	struct floppy_state *fs = (struct floppy_state *) dev_id;
658 	struct swim3 __iomem *sw = fs->swim3;
659 	int intr, err, n;
660 	int stat, resid;
661 	struct dbdma_regs __iomem *dr;
662 	struct dbdma_cmd *cp;
663 	unsigned long flags;
664 	struct request *req = fs->cur_req;
665 
666 	swim3_dbg("* interrupt, state=%d\n", fs->state);
667 
668 	spin_lock_irqsave(&swim3_lock, flags);
669 	intr = in_8(&sw->intr);
670 	err = (intr & ERROR_INTR)? in_8(&sw->error): 0;
671 	if ((intr & ERROR_INTR) && fs->state != do_transfer)
672 		swim3_err("Non-transfer error interrupt: state=%d, dir=%x, intr=%x, err=%x\n",
673 			  fs->state, rq_data_dir(req), intr, err);
674 	switch (fs->state) {
675 	case locating:
676 		if (intr & SEEN_SECTOR) {
677 			out_8(&sw->control_bic, DO_ACTION | WRITE_SECTORS);
678 			out_8(&sw->select, RELAX);
679 			out_8(&sw->intr_enable, 0);
680 			del_timer(&fs->timeout);
681 			fs->timeout_pending = 0;
682 			if (sw->ctrack == 0xff) {
683 				swim3_err("%s", "Seen sector but cyl=ff?\n");
684 				fs->cur_cyl = -1;
685 				if (fs->retries > 5) {
686 					swim3_end_request(fs, BLK_STS_IOERR, 0);
687 					fs->state = idle;
688 				} else {
689 					fs->state = jogging;
690 					act(fs);
691 				}
692 				break;
693 			}
694 			fs->cur_cyl = sw->ctrack;
695 			fs->cur_sector = sw->csect;
696 			if (fs->expect_cyl != -1 && fs->expect_cyl != fs->cur_cyl)
697 				swim3_err("Expected cyl %d, got %d\n",
698 					  fs->expect_cyl, fs->cur_cyl);
699 			fs->state = do_transfer;
700 			act(fs);
701 		}
702 		break;
703 	case seeking:
704 	case jogging:
705 		if (sw->nseek == 0) {
706 			out_8(&sw->control_bic, DO_SEEK);
707 			out_8(&sw->select, RELAX);
708 			out_8(&sw->intr_enable, 0);
709 			del_timer(&fs->timeout);
710 			fs->timeout_pending = 0;
711 			if (fs->state == seeking)
712 				++fs->retries;
713 			fs->state = settling;
714 			act(fs);
715 		}
716 		break;
717 	case settling:
718 		out_8(&sw->intr_enable, 0);
719 		del_timer(&fs->timeout);
720 		fs->timeout_pending = 0;
721 		act(fs);
722 		break;
723 	case do_transfer:
724 		if ((intr & (ERROR_INTR | TRANSFER_DONE)) == 0)
725 			break;
726 		out_8(&sw->intr_enable, 0);
727 		out_8(&sw->control_bic, WRITE_SECTORS | DO_ACTION);
728 		out_8(&sw->select, RELAX);
729 		del_timer(&fs->timeout);
730 		fs->timeout_pending = 0;
731 		dr = fs->dma;
732 		cp = fs->dma_cmd;
733 		if (rq_data_dir(req) == WRITE)
734 			++cp;
735 		/*
736 		 * Check that the main data transfer has finished.
737 		 * On writing, the swim3 sometimes doesn't use
738 		 * up all the bytes of the postamble, so we can still
739 		 * see DMA active here.  That doesn't matter as long
740 		 * as all the sector data has been transferred.
741 		 */
742 		if ((intr & ERROR_INTR) == 0 && cp->xfer_status == 0) {
743 			/* wait a little while for DMA to complete */
744 			for (n = 0; n < 100; ++n) {
745 				if (cp->xfer_status != 0)
746 					break;
747 				udelay(1);
748 				barrier();
749 			}
750 		}
751 		/* turn off DMA */
752 		out_le32(&dr->control, (RUN | PAUSE) << 16);
753 		stat = le16_to_cpu(cp->xfer_status);
754 		resid = le16_to_cpu(cp->res_count);
755 		if (intr & ERROR_INTR) {
756 			n = fs->scount - 1 - resid / 512;
757 			if (n > 0) {
758 				blk_update_request(req, 0, n << 9);
759 				fs->req_sector += n;
760 			}
761 			if (fs->retries < 5) {
762 				++fs->retries;
763 				act(fs);
764 			} else {
765 				swim3_err("Error %sing block %ld (err=%x)\n",
766 				       rq_data_dir(req) == WRITE? "writ": "read",
767 				       (long)blk_rq_pos(req), err);
768 				swim3_end_request(fs, BLK_STS_IOERR, 0);
769 				fs->state = idle;
770 			}
771 		} else {
772 			if ((stat & ACTIVE) == 0 || resid != 0) {
773 				/* musta been an error */
774 				swim3_err("fd dma error: stat=%x resid=%d\n", stat, resid);
775 				swim3_err("  state=%d, dir=%x, intr=%x, err=%x\n",
776 					  fs->state, rq_data_dir(req), intr, err);
777 				swim3_end_request(fs, BLK_STS_IOERR, 0);
778 				fs->state = idle;
779 				break;
780 			}
781 			fs->retries = 0;
782 			if (swim3_end_request(fs, 0, fs->scount << 9)) {
783 				fs->req_sector += fs->scount;
784 				if (fs->req_sector > fs->secpertrack) {
785 					fs->req_sector -= fs->secpertrack;
786 					if (++fs->head > 1) {
787 						fs->head = 0;
788 						++fs->req_cyl;
789 					}
790 				}
791 				act(fs);
792 			} else
793 				fs->state = idle;
794 		}
795 		break;
796 	default:
797 		swim3_err("Don't know what to do in state %d\n", fs->state);
798 	}
799 	spin_unlock_irqrestore(&swim3_lock, flags);
800 	return IRQ_HANDLED;
801 }
802 
803 /*
804 static void fd_dma_interrupt(int irq, void *dev_id)
805 {
806 }
807 */
808 
809 /* Called under the mutex to grab exclusive access to a drive */
grab_drive(struct floppy_state * fs,enum swim_state state,int interruptible)810 static int grab_drive(struct floppy_state *fs, enum swim_state state,
811 		      int interruptible)
812 {
813 	unsigned long flags;
814 
815 	swim3_dbg("%s", "-> grab drive\n");
816 
817 	spin_lock_irqsave(&swim3_lock, flags);
818 	if (fs->state != idle && fs->state != available) {
819 		++fs->wanted;
820 		/* this will enable irqs in order to sleep */
821 		if (!interruptible)
822 			wait_event_lock_irq(fs->wait,
823                                         fs->state == available,
824                                         swim3_lock);
825 		else if (wait_event_interruptible_lock_irq(fs->wait,
826 					fs->state == available,
827 					swim3_lock)) {
828 			--fs->wanted;
829 			spin_unlock_irqrestore(&swim3_lock, flags);
830 			return -EINTR;
831 		}
832 		--fs->wanted;
833 	}
834 	fs->state = state;
835 	spin_unlock_irqrestore(&swim3_lock, flags);
836 
837 	return 0;
838 }
839 
release_drive(struct floppy_state * fs)840 static void release_drive(struct floppy_state *fs)
841 {
842 	struct request_queue *q = disks[fs->index]->queue;
843 	unsigned long flags;
844 
845 	swim3_dbg("%s", "-> release drive\n");
846 
847 	spin_lock_irqsave(&swim3_lock, flags);
848 	fs->state = idle;
849 	spin_unlock_irqrestore(&swim3_lock, flags);
850 
851 	blk_mq_freeze_queue(q);
852 	blk_mq_quiesce_queue(q);
853 	blk_mq_unquiesce_queue(q);
854 	blk_mq_unfreeze_queue(q);
855 }
856 
fd_eject(struct floppy_state * fs)857 static int fd_eject(struct floppy_state *fs)
858 {
859 	int err, n;
860 
861 	err = grab_drive(fs, ejecting, 1);
862 	if (err)
863 		return err;
864 	swim3_action(fs, EJECT);
865 	for (n = 20; n > 0; --n) {
866 		if (signal_pending(current)) {
867 			err = -EINTR;
868 			break;
869 		}
870 		swim3_select(fs, RELAX);
871 		schedule_timeout_interruptible(1);
872 		if (swim3_readbit(fs, DISK_IN) == 0)
873 			break;
874 	}
875 	swim3_select(fs, RELAX);
876 	udelay(150);
877 	fs->ejected = 1;
878 	release_drive(fs);
879 	return err;
880 }
881 
882 static struct floppy_struct floppy_type =
883 	{ 2880,18,2,80,0,0x1B,0x00,0xCF,0x6C,NULL };	/*  7 1.44MB 3.5"   */
884 
floppy_locked_ioctl(struct block_device * bdev,blk_mode_t mode,unsigned int cmd,unsigned long param)885 static int floppy_locked_ioctl(struct block_device *bdev, blk_mode_t mode,
886 			unsigned int cmd, unsigned long param)
887 {
888 	struct floppy_state *fs = bdev->bd_disk->private_data;
889 	int err;
890 
891 	if ((cmd & 0x80) && !capable(CAP_SYS_ADMIN))
892 		return -EPERM;
893 
894 	if (fs->mdev->media_bay &&
895 	    check_media_bay(fs->mdev->media_bay) != MB_FD)
896 		return -ENXIO;
897 
898 	switch (cmd) {
899 	case FDEJECT:
900 		if (fs->ref_count != 1)
901 			return -EBUSY;
902 		err = fd_eject(fs);
903 		return err;
904 	case FDGETPRM:
905 	        if (copy_to_user((void __user *) param, &floppy_type,
906 				 sizeof(struct floppy_struct)))
907 			return -EFAULT;
908 		return 0;
909 	}
910 	return -ENOTTY;
911 }
912 
floppy_ioctl(struct block_device * bdev,blk_mode_t mode,unsigned int cmd,unsigned long param)913 static int floppy_ioctl(struct block_device *bdev, blk_mode_t mode,
914 				 unsigned int cmd, unsigned long param)
915 {
916 	int ret;
917 
918 	mutex_lock(&swim3_mutex);
919 	ret = floppy_locked_ioctl(bdev, mode, cmd, param);
920 	mutex_unlock(&swim3_mutex);
921 
922 	return ret;
923 }
924 
floppy_open(struct gendisk * disk,blk_mode_t mode)925 static int floppy_open(struct gendisk *disk, blk_mode_t mode)
926 {
927 	struct floppy_state *fs = disk->private_data;
928 	struct swim3 __iomem *sw = fs->swim3;
929 	int n, err = 0;
930 
931 	if (fs->ref_count == 0) {
932 		if (fs->mdev->media_bay &&
933 		    check_media_bay(fs->mdev->media_bay) != MB_FD)
934 			return -ENXIO;
935 		out_8(&sw->setup, S_IBM_DRIVE | S_FCLK_DIV2);
936 		out_8(&sw->control_bic, 0xff);
937 		out_8(&sw->mode, 0x95);
938 		udelay(10);
939 		out_8(&sw->intr_enable, 0);
940 		out_8(&sw->control_bis, DRIVE_ENABLE | INTR_ENABLE);
941 		swim3_action(fs, MOTOR_ON);
942 		fs->write_prot = -1;
943 		fs->cur_cyl = -1;
944 		for (n = 0; n < 2 * HZ; ++n) {
945 			if (n >= HZ/30 && swim3_readbit(fs, SEEK_COMPLETE))
946 				break;
947 			if (signal_pending(current)) {
948 				err = -EINTR;
949 				break;
950 			}
951 			swim3_select(fs, RELAX);
952 			schedule_timeout_interruptible(1);
953 		}
954 		if (err == 0 && (swim3_readbit(fs, SEEK_COMPLETE) == 0
955 				 || swim3_readbit(fs, DISK_IN) == 0))
956 			err = -ENXIO;
957 		swim3_action(fs, SETMFM);
958 		swim3_select(fs, RELAX);
959 
960 	} else if (fs->ref_count == -1 || mode & BLK_OPEN_EXCL)
961 		return -EBUSY;
962 
963 	if (err == 0 && !(mode & BLK_OPEN_NDELAY) &&
964 	    (mode & (BLK_OPEN_READ | BLK_OPEN_WRITE))) {
965 		if (disk_check_media_change(disk))
966 			floppy_revalidate(disk);
967 		if (fs->ejected)
968 			err = -ENXIO;
969 	}
970 
971 	if (err == 0 && (mode & BLK_OPEN_WRITE)) {
972 		if (fs->write_prot < 0)
973 			fs->write_prot = swim3_readbit(fs, WRITE_PROT);
974 		if (fs->write_prot)
975 			err = -EROFS;
976 	}
977 
978 	if (err) {
979 		if (fs->ref_count == 0) {
980 			swim3_action(fs, MOTOR_OFF);
981 			out_8(&sw->control_bic, DRIVE_ENABLE | INTR_ENABLE);
982 			swim3_select(fs, RELAX);
983 		}
984 		return err;
985 	}
986 
987 	if (mode & BLK_OPEN_EXCL)
988 		fs->ref_count = -1;
989 	else
990 		++fs->ref_count;
991 
992 	return 0;
993 }
994 
floppy_unlocked_open(struct gendisk * disk,blk_mode_t mode)995 static int floppy_unlocked_open(struct gendisk *disk, blk_mode_t mode)
996 {
997 	int ret;
998 
999 	mutex_lock(&swim3_mutex);
1000 	ret = floppy_open(disk, mode);
1001 	mutex_unlock(&swim3_mutex);
1002 
1003 	return ret;
1004 }
1005 
floppy_release(struct gendisk * disk)1006 static void floppy_release(struct gendisk *disk)
1007 {
1008 	struct floppy_state *fs = disk->private_data;
1009 	struct swim3 __iomem *sw = fs->swim3;
1010 
1011 	mutex_lock(&swim3_mutex);
1012 	if (fs->ref_count > 0)
1013 		--fs->ref_count;
1014 	else if (fs->ref_count == -1)
1015 		fs->ref_count = 0;
1016 	if (fs->ref_count == 0) {
1017 		swim3_action(fs, MOTOR_OFF);
1018 		out_8(&sw->control_bic, 0xff);
1019 		swim3_select(fs, RELAX);
1020 	}
1021 	mutex_unlock(&swim3_mutex);
1022 }
1023 
floppy_check_events(struct gendisk * disk,unsigned int clearing)1024 static unsigned int floppy_check_events(struct gendisk *disk,
1025 					unsigned int clearing)
1026 {
1027 	struct floppy_state *fs = disk->private_data;
1028 	return fs->ejected ? DISK_EVENT_MEDIA_CHANGE : 0;
1029 }
1030 
floppy_revalidate(struct gendisk * disk)1031 static int floppy_revalidate(struct gendisk *disk)
1032 {
1033 	struct floppy_state *fs = disk->private_data;
1034 	struct swim3 __iomem *sw;
1035 	int ret, n;
1036 
1037 	if (fs->mdev->media_bay &&
1038 	    check_media_bay(fs->mdev->media_bay) != MB_FD)
1039 		return -ENXIO;
1040 
1041 	sw = fs->swim3;
1042 	grab_drive(fs, revalidating, 0);
1043 	out_8(&sw->intr_enable, 0);
1044 	out_8(&sw->control_bis, DRIVE_ENABLE);
1045 	swim3_action(fs, MOTOR_ON);	/* necessary? */
1046 	fs->write_prot = -1;
1047 	fs->cur_cyl = -1;
1048 	mdelay(1);
1049 	for (n = HZ; n > 0; --n) {
1050 		if (swim3_readbit(fs, SEEK_COMPLETE))
1051 			break;
1052 		if (signal_pending(current))
1053 			break;
1054 		swim3_select(fs, RELAX);
1055 		schedule_timeout_interruptible(1);
1056 	}
1057 	ret = swim3_readbit(fs, SEEK_COMPLETE) == 0
1058 		|| swim3_readbit(fs, DISK_IN) == 0;
1059 	if (ret)
1060 		swim3_action(fs, MOTOR_OFF);
1061 	else {
1062 		fs->ejected = 0;
1063 		swim3_action(fs, SETMFM);
1064 	}
1065 	swim3_select(fs, RELAX);
1066 
1067 	release_drive(fs);
1068 	return ret;
1069 }
1070 
1071 static const struct block_device_operations floppy_fops = {
1072 	.open		= floppy_unlocked_open,
1073 	.release	= floppy_release,
1074 	.ioctl		= floppy_ioctl,
1075 	.check_events	= floppy_check_events,
1076 };
1077 
1078 static const struct blk_mq_ops swim3_mq_ops = {
1079 	.queue_rq = swim3_queue_rq,
1080 };
1081 
swim3_mb_event(struct macio_dev * mdev,int mb_state)1082 static void swim3_mb_event(struct macio_dev* mdev, int mb_state)
1083 {
1084 	struct floppy_state *fs = macio_get_drvdata(mdev);
1085 	struct swim3 __iomem *sw;
1086 
1087 	if (!fs)
1088 		return;
1089 
1090 	sw = fs->swim3;
1091 
1092 	if (mb_state != MB_FD)
1093 		return;
1094 
1095 	/* Clear state */
1096 	out_8(&sw->intr_enable, 0);
1097 	in_8(&sw->intr);
1098 	in_8(&sw->error);
1099 }
1100 
swim3_add_device(struct macio_dev * mdev,int index)1101 static int swim3_add_device(struct macio_dev *mdev, int index)
1102 {
1103 	struct device_node *swim = mdev->ofdev.dev.of_node;
1104 	struct floppy_state *fs = &floppy_states[index];
1105 	int rc = -EBUSY;
1106 
1107 	fs->mdev = mdev;
1108 	fs->index = index;
1109 
1110 	/* Check & Request resources */
1111 	if (macio_resource_count(mdev) < 2) {
1112 		swim3_err("%s", "No address in device-tree\n");
1113 		return -ENXIO;
1114 	}
1115 	if (macio_irq_count(mdev) < 1) {
1116 		swim3_err("%s", "No interrupt in device-tree\n");
1117 		return -ENXIO;
1118 	}
1119 	if (macio_request_resource(mdev, 0, "swim3 (mmio)")) {
1120 		swim3_err("%s", "Can't request mmio resource\n");
1121 		return -EBUSY;
1122 	}
1123 	if (macio_request_resource(mdev, 1, "swim3 (dma)")) {
1124 		swim3_err("%s", "Can't request dma resource\n");
1125 		macio_release_resource(mdev, 0);
1126 		return -EBUSY;
1127 	}
1128 	dev_set_drvdata(&mdev->ofdev.dev, fs);
1129 
1130 	if (mdev->media_bay == NULL)
1131 		pmac_call_feature(PMAC_FTR_SWIM3_ENABLE, swim, 0, 1);
1132 
1133 	fs->state = idle;
1134 	fs->swim3 = (struct swim3 __iomem *)
1135 		ioremap(macio_resource_start(mdev, 0), 0x200);
1136 	if (fs->swim3 == NULL) {
1137 		swim3_err("%s", "Couldn't map mmio registers\n");
1138 		rc = -ENOMEM;
1139 		goto out_release;
1140 	}
1141 	fs->dma = (struct dbdma_regs __iomem *)
1142 		ioremap(macio_resource_start(mdev, 1), 0x200);
1143 	if (fs->dma == NULL) {
1144 		swim3_err("%s", "Couldn't map dma registers\n");
1145 		iounmap(fs->swim3);
1146 		rc = -ENOMEM;
1147 		goto out_release;
1148 	}
1149 	fs->swim3_intr = macio_irq(mdev, 0);
1150 	fs->dma_intr = macio_irq(mdev, 1);
1151 	fs->cur_cyl = -1;
1152 	fs->cur_sector = -1;
1153 	fs->secpercyl = 36;
1154 	fs->secpertrack = 18;
1155 	fs->total_secs = 2880;
1156 	init_waitqueue_head(&fs->wait);
1157 
1158 	fs->dma_cmd = (struct dbdma_cmd *) DBDMA_ALIGN(fs->dbdma_cmd_space);
1159 	memset(fs->dma_cmd, 0, 2 * sizeof(struct dbdma_cmd));
1160 	fs->dma_cmd[1].command = cpu_to_le16(DBDMA_STOP);
1161 
1162 	if (mdev->media_bay == NULL || check_media_bay(mdev->media_bay) == MB_FD)
1163 		swim3_mb_event(mdev, MB_FD);
1164 
1165 	if (request_irq(fs->swim3_intr, swim3_interrupt, 0, "SWIM3", fs)) {
1166 		swim3_err("%s", "Couldn't request interrupt\n");
1167 		pmac_call_feature(PMAC_FTR_SWIM3_ENABLE, swim, 0, 0);
1168 		goto out_unmap;
1169 	}
1170 
1171 	timer_setup(&fs->timeout, NULL, 0);
1172 
1173 	swim3_info("SWIM3 floppy controller %s\n",
1174 		mdev->media_bay ? "in media bay" : "");
1175 
1176 	return 0;
1177 
1178  out_unmap:
1179 	iounmap(fs->dma);
1180 	iounmap(fs->swim3);
1181 
1182  out_release:
1183 	macio_release_resource(mdev, 0);
1184 	macio_release_resource(mdev, 1);
1185 
1186 	return rc;
1187 }
1188 
swim3_attach(struct macio_dev * mdev,const struct of_device_id * match)1189 static int swim3_attach(struct macio_dev *mdev,
1190 			const struct of_device_id *match)
1191 {
1192 	struct floppy_state *fs;
1193 	struct gendisk *disk;
1194 	int rc;
1195 
1196 	if (floppy_count >= MAX_FLOPPIES)
1197 		return -ENXIO;
1198 
1199 	if (floppy_count == 0) {
1200 		rc = register_blkdev(FLOPPY_MAJOR, "fd");
1201 		if (rc)
1202 			return rc;
1203 	}
1204 
1205 	fs = &floppy_states[floppy_count];
1206 	memset(fs, 0, sizeof(*fs));
1207 
1208 	rc = blk_mq_alloc_sq_tag_set(&fs->tag_set, &swim3_mq_ops, 2,
1209 			BLK_MQ_F_SHOULD_MERGE);
1210 	if (rc)
1211 		goto out_unregister;
1212 
1213 	disk = blk_mq_alloc_disk(&fs->tag_set, fs);
1214 	if (IS_ERR(disk)) {
1215 		rc = PTR_ERR(disk);
1216 		goto out_free_tag_set;
1217 	}
1218 
1219 	rc = swim3_add_device(mdev, floppy_count);
1220 	if (rc)
1221 		goto out_cleanup_disk;
1222 
1223 	disk->major = FLOPPY_MAJOR;
1224 	disk->first_minor = floppy_count;
1225 	disk->minors = 1;
1226 	disk->fops = &floppy_fops;
1227 	disk->private_data = fs;
1228 	disk->events = DISK_EVENT_MEDIA_CHANGE;
1229 	disk->flags |= GENHD_FL_REMOVABLE | GENHD_FL_NO_PART;
1230 	sprintf(disk->disk_name, "fd%d", floppy_count);
1231 	set_capacity(disk, 2880);
1232 	rc = add_disk(disk);
1233 	if (rc)
1234 		goto out_cleanup_disk;
1235 
1236 	disks[floppy_count++] = disk;
1237 	return 0;
1238 
1239 out_cleanup_disk:
1240 	put_disk(disk);
1241 out_free_tag_set:
1242 	blk_mq_free_tag_set(&fs->tag_set);
1243 out_unregister:
1244 	if (floppy_count == 0)
1245 		unregister_blkdev(FLOPPY_MAJOR, "fd");
1246 	return rc;
1247 }
1248 
1249 static const struct of_device_id swim3_match[] =
1250 {
1251 	{
1252 	.name		= "swim3",
1253 	},
1254 	{
1255 	.compatible	= "ohare-swim3"
1256 	},
1257 	{
1258 	.compatible	= "swim3"
1259 	},
1260 	{ /* end of list */ }
1261 };
1262 
1263 static struct macio_driver swim3_driver =
1264 {
1265 	.driver = {
1266 		.name 		= "swim3",
1267 		.of_match_table	= swim3_match,
1268 	},
1269 	.probe		= swim3_attach,
1270 #ifdef CONFIG_PMAC_MEDIABAY
1271 	.mediabay_event	= swim3_mb_event,
1272 #endif
1273 #if 0
1274 	.suspend	= swim3_suspend,
1275 	.resume		= swim3_resume,
1276 #endif
1277 };
1278 
1279 
swim3_init(void)1280 static int swim3_init(void)
1281 {
1282 	macio_register_driver(&swim3_driver);
1283 	return 0;
1284 }
1285 
1286 module_init(swim3_init)
1287 
1288 MODULE_LICENSE("GPL");
1289 MODULE_AUTHOR("Paul Mackerras");
1290 MODULE_ALIAS_BLOCKDEV_MAJOR(FLOPPY_MAJOR);
1291