xref: /linux/drivers/block/swim.c (revision cb8fef4b07f71da01843457b66e579ad248a625e)
1 // SPDX-License-Identifier: GPL-2.0-or-later
2 /*
3  * Driver for SWIM (Sander Woz Integrated Machine) floppy controller
4  *
5  * Copyright (C) 2004, 2008 Laurent Vivier <Laurent@lvivier.info>
6  *
7  * based on Alastair Bridgewater SWIM analysis, 2001
8  * based on SWIM3 driver (c) Paul Mackerras, 1996
9  * based on netBSD IWM driver (c) 1997, 1998 Hauke Fath.
10  *
11  * See also:
12  * Inside Macintosh, vol. III, ch. 2
13  * https://archive.org/details/SWIMDesignDocs
14  * NEC uPD72070 FDC datasheet
15  * mkLinux source file swimiiicommonhal.c
16  * http://www.mac.linux-m68k.org/devel/iwm.php.html
17  * MAME source file sonydriv.cpp
18  *
19  * 2004-08-21 (lv) - Initial implementation
20  * 2008-10-30 (lv) - Port to 2.6
21  */
22 
23 #include <linux/module.h>
24 #include <linux/fd.h>
25 #include <linux/slab.h>
26 #include <linux/blk-mq.h>
27 #include <linux/major.h>
28 #include <linux/mutex.h>
29 #include <linux/hdreg.h>
30 #include <linux/iopoll.h>
31 #include <linux/kernel.h>
32 #include <linux/delay.h>
33 #include <linux/platform_device.h>
34 
35 #include <asm/mac_via.h>
36 
37 #define CARDNAME "swim"
38 
39 struct sector_header {
40 	unsigned char side;
41 	unsigned char track;
42 	unsigned char sector;
43 	unsigned char size;
44 	unsigned char crc0;
45 	unsigned char crc1;
46 } __attribute__((packed));
47 
48 #define DRIVER_VERSION "Version 0.2 (2008-10-30)"
49 
50 #define REG(x)	unsigned char x, x ## _pad[0x200 - 1];
51 
52 struct swim {
53 	REG(write_data)
54 	REG(write_mark)
55 	REG(write_CRC)
56 	REG(write_parameter)
57 	REG(write_phase)
58 	REG(write_setup)
59 	REG(write_mode0)
60 	REG(write_mode1)
61 
62 	REG(read_data)
63 	REG(read_mark)
64 	REG(read_error)
65 	REG(read_parameter)
66 	REG(read_phase)
67 	REG(read_setup)
68 	REG(read_status)
69 	REG(read_handshake)
70 } __attribute__((packed));
71 
72 #define swim_write(base, reg, v)	out_8(&(base)->write_##reg, (v))
73 #define swim_read(base, reg)		in_8(&(base)->read_##reg)
74 
75 /* IWM registers */
76 
77 struct iwm {
78 	REG(ph0L)
79 	REG(ph0H)
80 	REG(ph1L)
81 	REG(ph1H)
82 	REG(ph2L)
83 	REG(ph2H)
84 	REG(ph3L)
85 	REG(ph3H)
86 	REG(mtrOff)
87 	REG(mtrOn)
88 	REG(intDrive)
89 	REG(extDrive)
90 	REG(q6L)
91 	REG(q6H)
92 	REG(q7L)
93 	REG(q7H)
94 } __attribute__((packed));
95 
96 #define iwm_write(base, reg, v)		out_8(&(base)->reg, (v))
97 #define iwm_read(base, reg)		in_8(&(base)->reg)
98 
99 /* Bits in phase register */
100 
101 #define RELAX		0x03
102 #define LSTRB		0x08
103 #define CA_MASK		0x07
104 #define PHASE_PIN_DIR	0xF0
105 
106 /* Select values for swim_select and swim_readbit */
107 
108 #define SEL_MASK	0x100
109 
110 #define SEEK_POSITIVE	0x000
111 #define SEEK_NEGATIVE	0x004
112 #define STEP		0x001
113 #define MOTOR_ON	0x002
114 #define MOTOR_OFF	0x006
115 #define INDEX		0x003
116 #define EJECT		0x007
117 #define SETMFM		0x101
118 #define SETGCR		0x105
119 
120 #define STEPPING	0x001
121 #define READ_DATA_0	0x004
122 #define ONEMEG_DRIVE	0x005
123 #define SINGLE_SIDED	0x006
124 #define DRIVE_PRESENT	0x007
125 #define DISK_IN		0x100
126 #define WRITE_PROT	0x101
127 #define TRACK_ZERO	0x102
128 #define TACHO		0x103
129 #define READ_DATA_1	0x104
130 #define GCR_MODE	0x105
131 #define READY		0x106
132 #define TWOMEG_MEDIA	0x107
133 
134 /* Bits in handshake register */
135 
136 #define SENSE		0x08
137 #define DAT2BYTE	0x40
138 #define DAT1BYTE	0x80
139 
140 /* bits in setup register */
141 
142 #define S_FCLK_DIV2	0x08
143 #define S_IBM_DRIVE	0x20
144 
145 /* bits in mode register */
146 
147 #define CLFIFO		0x01
148 #define ENBL1		0x02
149 #define ENBL2		0x04
150 #define ACTION		0x08
151 #define WRITE_MODE	0x10
152 #define ISM_SELECT	0x40
153 #define MOTON		0x80
154 
155 enum drive_location {
156 	NO_DRIVE = 0,
157 	INTERNAL_DRIVE = BIT(1),
158 	EXTERNAL_DRIVE = BIT(2),
159 };
160 
161 enum media_type {
162 	DD_MEDIA,
163 	HD_MEDIA,
164 };
165 
166 struct floppy_state {
167 
168 	/* physical properties */
169 
170 	enum drive_location location;	/* internal or external drive */
171 	int head_number;		/* single- or double-sided drive */
172 
173 	/* media */
174 
175 	int disk_in;
176 	int ejected;
177 	enum media_type type;
178 	int write_protected;
179 
180 	unsigned int total_secs;
181 	unsigned int secpercyl;
182 	unsigned int secpertrack;
183 
184 	/* in-use information */
185 
186 	int track;
187 	int ref_count;
188 	bool registered;
189 
190 	struct gendisk *disk;
191 	struct blk_mq_tag_set tag_set;
192 
193 	/* parent controller */
194 
195 	struct swim_priv *swd;
196 };
197 
198 enum motor_action {
199 	OFF,
200 	ON,
201 };
202 
203 enum head {
204 	LOWER_HEAD = 0,
205 	UPPER_HEAD = 1,
206 };
207 
208 #define FD_MAX_UNIT	2
209 
210 struct swim_priv {
211 	struct swim __iomem *base;
212 	int floppy_count;
213 	struct floppy_state unit[FD_MAX_UNIT];
214 };
215 
216 extern int swim_read_sector_header(struct swim __iomem *base,
217 				   struct sector_header *header);
218 extern int swim_read_sector_data(struct swim __iomem *base,
219 				 unsigned char *data);
220 
221 static DEFINE_MUTEX(swim_mutex);
222 
223 static void set_swim_mode(struct swim __iomem *base, int enable)
224 {
225 	struct iwm __iomem *iwm_base = (struct iwm __iomem *)base;
226 	unsigned long flags;
227 
228 	if (!enable) {
229 		swim_write(base, mode0, ACTION);
230 		swim_write(base, mode0, ENBL1 | ENBL2 | MOTON);
231 		swim_write(base, mode0, ISM_SELECT);
232 		iwm_read(iwm_base, q7L);
233 		return;
234 	}
235 
236 	local_irq_save(flags);
237 
238 	iwm_read(iwm_base, q7L);
239 	iwm_read(iwm_base, q6L);
240 	iwm_read(iwm_base, mtrOff);
241 	iwm_read(iwm_base, q6H);
242 
243 	iwm_write(iwm_base, q7H, 0x57);
244 	iwm_write(iwm_base, q7H, 0x17);
245 	iwm_write(iwm_base, q7H, 0x57);
246 	iwm_write(iwm_base, q7H, 0x57);
247 
248 	local_irq_restore(flags);
249 }
250 
251 static int get_swim_mode(struct swim __iomem *base)
252 {
253 	unsigned long flags;
254 
255 	local_irq_save(flags);
256 
257 	swim_write(base, phase, 0xf5);
258 	if (swim_read(base, phase) != 0xf5)
259 		goto is_iwm;
260 	swim_write(base, phase, 0xf6);
261 	if (swim_read(base, phase) != 0xf6)
262 		goto is_iwm;
263 	swim_write(base, phase, 0xf7);
264 	if (swim_read(base, phase) != 0xf7)
265 		goto is_iwm;
266 	local_irq_restore(flags);
267 	return 1;
268 is_iwm:
269 	local_irq_restore(flags);
270 	return 0;
271 }
272 
273 static inline void swim_select(struct swim __iomem *base, int sel)
274 {
275 	swim_write(base, phase, RELAX | PHASE_PIN_DIR);
276 
277 	via1_set_head(sel & SEL_MASK);
278 
279 	swim_write(base, phase, (sel & CA_MASK) | PHASE_PIN_DIR);
280 }
281 
282 static inline void swim_action(struct swim __iomem *base, int action)
283 {
284 	unsigned long flags;
285 
286 	local_irq_save(flags);
287 
288 	swim_select(base, action);
289 	udelay(1);
290 	swim_write(base, phase, LSTRB | action | PHASE_PIN_DIR);
291 	udelay(2);
292 	swim_write(base, phase, action | PHASE_PIN_DIR);
293 	udelay(1);
294 
295 	local_irq_restore(flags);
296 }
297 
298 static inline bool swim_readbit(struct swim __iomem *base, int bit)
299 {
300 	int stat;
301 
302 	swim_select(base, bit);
303 
304 	udelay(10);
305 
306 	stat = swim_read(base, handshake);
307 
308 	return (stat & SENSE) == 0;
309 }
310 
311 #define swim_readbit_timeout(base, bit, val, timeout_us) \
312 	poll_timeout_us(, swim_readbit(base, bit) == val, 1000, timeout_us, false)
313 
314 #define swim_READY_timeout(base) \
315 ({ \
316 	int ret = swim_readbit_timeout(base, READY, true, 1000 * 1000); \
317 	if (ret) \
318 		printk(KERN_DEBUG "%s: drive not ready\n", __func__); \
319 	ret; \
320 })
321 
322 static void swim_drive(struct swim __iomem *base,
323 			      enum drive_location location)
324 {
325 	if (location == INTERNAL_DRIVE) {
326 		swim_write(base, mode0, EXTERNAL_DRIVE); /* clear drive 1 bit */
327 		swim_write(base, mode1, INTERNAL_DRIVE); /* set drive 0 bit */
328 		swim_write(base, mode1, MOTON);
329 	} else if (location == EXTERNAL_DRIVE) {
330 		swim_write(base, mode0, INTERNAL_DRIVE); /* clear drive 0 bit */
331 		swim_write(base, mode1, EXTERNAL_DRIVE); /* set drive 1 bit */
332 		swim_write(base, mode1, MOTON);
333 	} else {
334 		swim_write(base, mode0, INTERNAL_DRIVE);
335 		swim_write(base, mode0, EXTERNAL_DRIVE);
336 		swim_write(base, mode0, MOTON);
337 	}
338 	udelay(1);
339 }
340 
341 static void swim_motor(struct swim __iomem *base,
342 		       enum motor_action action)
343 {
344 	if (action == ON) {
345 		swim_action(base, MOTOR_ON);
346 		swim_readbit_timeout(base, MOTOR_ON, true, 2000 * 1000);
347 	} else if (action == OFF) {
348 		swim_action(base, MOTOR_OFF);
349 		swim_write(base, phase, RELAX | PHASE_PIN_DIR);
350 	}
351 }
352 
353 static void swim_eject(struct swim __iomem *base)
354 {
355 	swim_action(base, EJECT);
356 	swim_readbit_timeout(base, DISK_IN, false, 2000 * 1000);
357 	msleep(1);
358 }
359 
360 static inline void swim_head(struct swim __iomem *base, enum head head)
361 {
362 	if (head == UPPER_HEAD)
363 		swim_select(base, READ_DATA_1);
364 	else if (head == LOWER_HEAD)
365 		swim_select(base, READ_DATA_0);
366 }
367 
368 static int swim_step(struct swim __iomem *base)
369 {
370 	swim_action(base, STEP);
371 	udelay(150);
372 	return swim_readbit_timeout(base, STEPPING, false, 20 * 1000);
373 }
374 
375 static int swim_track00(struct swim __iomem *base)
376 {
377 	int try;
378 
379 	swim_action(base, SEEK_NEGATIVE);
380 
381 	for (try = 0; try < 100; try++) {
382 		msleep(3);
383 
384 		if (swim_readbit(base, TRACK_ZERO))
385 			return 0;
386 
387 		if (swim_step(base))
388 			break;
389 	}
390 
391 	pr_err("swim: track zero recalibration failed\n");
392 	return -1;
393 }
394 
395 static int swim_seek(struct swim __iomem *base, int step)
396 {
397 	if (step < 0) {
398 		swim_action(base, SEEK_NEGATIVE);
399 		step = -step;
400 	} else if (step > 0)
401 		swim_action(base, SEEK_POSITIVE);
402 	udelay(1);
403 
404 	swim_READY_timeout(base);
405 
406 	if (step == 0)
407 		return 0;
408 
409 	for (; step > 0; step--) {
410 		if (swim_step(base))
411 			return -1;
412 	}
413 
414 	msleep(30);
415 	swim_READY_timeout(base);
416 
417 	return 0;
418 }
419 
420 static int swim_track(struct floppy_state *fs, int track)
421 {
422 	struct swim __iomem *base = fs->swd->base;
423 	int ret;
424 
425 	ret = swim_seek(base, track - fs->track);
426 
427 	if (ret == 0)
428 		fs->track = track;
429 	else {
430 		swim_track00(base);
431 		fs->track = 0;
432 	}
433 
434 	return ret;
435 }
436 
437 static int floppy_eject(struct floppy_state *fs)
438 {
439 	struct swim __iomem *base = fs->swd->base;
440 
441 	swim_drive(base, fs->location);
442 	swim_track(fs, 40);
443 	swim_motor(base, OFF);
444 	swim_eject(base);
445 	swim_drive(base, NO_DRIVE);
446 
447 	fs->disk_in = 0;
448 	fs->ejected = 1;
449 
450 	return 0;
451 }
452 
453 static unsigned int swim_read_sector_range(struct floppy_state *fs,
454 					unsigned int side, unsigned int track,
455 					unsigned int start, unsigned int count,
456 					unsigned char *buffer)
457 {
458 	struct swim __iomem *base = fs->swd->base;
459 	unsigned long flags;
460 	struct sector_header header;
461 	unsigned int i, bits = 0;
462 
463 	if (count > 0) {
464 		count = min(count, fs->secpertrack);
465 		bits = GENMASK(count - 1, 0);
466 	}
467 
468 	local_irq_save(flags);
469 	for (i = 0; i < fs->secpertrack + 1; i++) {
470 		if (bits == 0) /* All sectors were read ok */
471 			break;
472 
473 		if (swim_read_sector_header(base, &header) == 0 &&
474 		    swim_read(base, error) == 0) {
475 			unsigned int offset = header.sector - start;
476 			unsigned char *buf = NULL;
477 			int len;
478 
479 			if (header.track == track && header.side == side &&
480 			    header.size == 2 && header.sector >= start &&
481 			    header.sector < start + count &&
482 			    (bits & BIT(offset)))
483 				buf = buffer + 512 * offset;
484 			len = swim_read_sector_data(base, buf);
485 			if (swim_read(base, error) == 0 && buf && len == 512)
486 				bits &= ~BIT(offset);
487 		}
488 	}
489 	local_irq_restore(flags);
490 
491 	return bits ? ffs(bits) - 1 : count; /* No. of contiguous ok sectors */
492 }
493 
494 static blk_status_t floppy_read_sectors(struct floppy_state *fs,
495 					unsigned int req_sector,
496 					unsigned int sectors_nb,
497 					unsigned char *buffer)
498 {
499 	struct swim __iomem *base = fs->swd->base;
500 	unsigned int try = 0;
501 
502 	swim_drive(base, fs->location);
503 	swim_READY_timeout(base);
504 
505 	while (sectors_nb) {
506 		unsigned int cyl, x, head, sector, n, n_ok;
507 
508 		cyl = req_sector / fs->secpercyl;
509 		x = req_sector % fs->secpercyl;
510 		head = (x >= fs->secpertrack) ? 1 : 0;
511 		sector = x % fs->secpertrack;
512 		n = min(sectors_nb, fs->secpertrack - sector);
513 
514 		swim_track(fs, cyl);
515 		swim_head(base, head);
516 
517 		n_ok = swim_read_sector_range(fs, head, cyl, sector + 1, n, buffer);
518 		if (n_ok == n)
519 			try = 0;
520 		else if (++try >= 5)
521 			return BLK_STS_IOERR;
522 
523 		buffer += 512 * n_ok;
524 		sectors_nb -= n_ok;
525 		req_sector += n_ok;
526 	}
527 
528 	return 0;
529 }
530 
531 static blk_status_t swim_queue_rq(struct blk_mq_hw_ctx *hctx,
532 				  const struct blk_mq_queue_data *bd)
533 {
534 	struct floppy_state *fs = hctx->queue->queuedata;
535 	struct request *req = bd->rq;
536 	blk_status_t err;
537 
538 	mutex_lock(&swim_mutex);
539 
540 	blk_mq_start_request(req);
541 
542 	if (!fs->disk_in || rq_data_dir(req) == WRITE) {
543 		err = BLK_STS_IOERR;
544 		goto out;
545 	}
546 
547 	do {
548 		err = floppy_read_sectors(fs, blk_rq_pos(req),
549 					  blk_rq_cur_sectors(req),
550 					  bio_data(req->bio));
551 	} while (blk_update_request(req, err, blk_rq_cur_bytes(req)));
552 	__blk_mq_end_request(req, err);
553 
554 	err = BLK_STS_OK;
555 out:
556 	mutex_unlock(&swim_mutex);
557 	return err;
558 
559 }
560 
561 static struct floppy_struct floppy_type[4] = {
562 	{    0,  0, 0,  0, 0, 0x00, 0x00, 0x00, 0x00, NULL }, /* no testing */
563 	{  720,  9, 1, 80, 0, 0x2A, 0x02, 0xDF, 0x50, NULL }, /* 360KB SS 3.5"*/
564 	{ 1440,  9, 2, 80, 0, 0x2A, 0x02, 0xDF, 0x50, NULL }, /* 720KB 3.5" */
565 	{ 2880, 18, 2, 80, 0, 0x1B, 0x00, 0xCF, 0x6C, NULL }, /* 1.44MB 3.5" */
566 };
567 
568 static int get_floppy_geometry(struct floppy_state *fs, int type,
569 			       struct floppy_struct **g)
570 {
571 	if (type >= ARRAY_SIZE(floppy_type))
572 		return -EINVAL;
573 
574 	if (type)
575 		*g = &floppy_type[type];
576 	else if (fs->type == HD_MEDIA) /* High-Density media */
577 		*g = &floppy_type[3];
578 	else if (fs->head_number == 2) /* double-sided */
579 		*g = &floppy_type[2];
580 	else
581 		*g = &floppy_type[1];
582 
583 	return 0;
584 }
585 
586 static void setup_medium(struct floppy_state *fs)
587 {
588 	struct swim __iomem *base = fs->swd->base;
589 
590 	if (swim_readbit(base, DISK_IN)) {
591 		struct floppy_struct *g;
592 
593 		fs->disk_in = 1;
594 		fs->write_protected = swim_readbit(base, WRITE_PROT);
595 		fs->type = swim_readbit(base, TWOMEG_MEDIA) ?
596 			   HD_MEDIA : DD_MEDIA;
597 		fs->head_number = swim_readbit(base, SINGLE_SIDED) ? 1 : 2;
598 		get_floppy_geometry(fs, 0, &g);
599 		fs->total_secs = g->size;
600 		fs->secpercyl = g->head * g->sect;
601 		fs->secpertrack = g->sect;
602 	} else {
603 		fs->disk_in = 0;
604 	}
605 }
606 
607 static int floppy_open(struct gendisk *disk, blk_mode_t mode)
608 {
609 	struct floppy_state *fs = disk->private_data;
610 	struct swim __iomem *base = fs->swd->base;
611 	int err;
612 
613 	if (fs->ref_count == -1 || (fs->ref_count && mode & BLK_OPEN_EXCL))
614 		return -EBUSY;
615 	if (mode & BLK_OPEN_EXCL)
616 		fs->ref_count = -1;
617 	else
618 		fs->ref_count++;
619 
620 	swim_drive(base, fs->location);
621 
622 	if (fs->ejected)
623 		setup_medium(fs);
624 	if (!fs->disk_in) {
625 		err = -ENXIO;
626 		goto out;
627 	}
628 
629 	swim_motor(base, ON);
630 	swim_action(base, SETMFM);
631 	msleep(30);
632 	swim_READY_timeout(base);
633 
634 	set_capacity(fs->disk, fs->total_secs);
635 
636 	if (mode & BLK_OPEN_NDELAY)
637 		return 0;
638 
639 	if (mode & (BLK_OPEN_READ | BLK_OPEN_WRITE)) {
640 		if (disk_check_media_change(disk) && fs->disk_in)
641 			fs->ejected = 0;
642 		if (mode & BLK_OPEN_WRITE) {
643 			/* Write support isn't implemented. */
644 			err = -EROFS;
645 			goto out;
646 		}
647 	}
648 	return 0;
649 out:
650 	if (fs->ref_count < 0)
651 		fs->ref_count = 0;
652 	else if (fs->ref_count > 0)
653 		--fs->ref_count;
654 
655 	if (fs->ref_count == 0) {
656 		swim_motor(base, OFF);
657 		swim_drive(base, NO_DRIVE);
658 	}
659 	return err;
660 }
661 
662 static int floppy_unlocked_open(struct gendisk *disk, blk_mode_t mode)
663 {
664 	int ret;
665 
666 	mutex_lock(&swim_mutex);
667 	ret = floppy_open(disk, mode);
668 	mutex_unlock(&swim_mutex);
669 
670 	return ret;
671 }
672 
673 static void floppy_release(struct gendisk *disk)
674 {
675 	struct floppy_state *fs = disk->private_data;
676 	struct swim __iomem *base = fs->swd->base;
677 
678 	mutex_lock(&swim_mutex);
679 	if (fs->ref_count < 0)
680 		fs->ref_count = 0;
681 	else if (fs->ref_count > 0)
682 		--fs->ref_count;
683 
684 	if (fs->ref_count == 0) {
685 		swim_drive(base, fs->location);
686 		swim_motor(base, OFF);
687 		swim_drive(base, NO_DRIVE);
688 	}
689 	mutex_unlock(&swim_mutex);
690 }
691 
692 static int floppy_ioctl(struct block_device *bdev, blk_mode_t mode,
693 			unsigned int cmd, unsigned long param)
694 {
695 	struct floppy_state *fs = bdev->bd_disk->private_data;
696 	int err;
697 
698 	if ((cmd & 0x80) && !capable(CAP_SYS_ADMIN))
699 		return -EPERM;
700 
701 	switch (cmd) {
702 	case FDEJECT:
703 		mutex_lock(&swim_mutex);
704 		if (fs->ref_count == -1 || fs->ref_count == 1)
705 			err = floppy_eject(fs);
706 		else
707 			err = -EBUSY;
708 		mutex_unlock(&swim_mutex);
709 		return err;
710 
711 	case FDGETPRM:
712 		if (copy_to_user((void __user *) param, (void *) &floppy_type,
713 				 sizeof(struct floppy_struct)))
714 			return -EFAULT;
715 		return 0;
716 	}
717 	return -ENOTTY;
718 }
719 
720 static int floppy_getgeo(struct gendisk *disk, struct hd_geometry *geo)
721 {
722 	struct floppy_state *fs = disk->private_data;
723 	struct floppy_struct *g;
724 	int ret;
725 
726 	ret = get_floppy_geometry(fs, 0, &g);
727 	if (ret)
728 		return ret;
729 
730 	geo->heads = g->head;
731 	geo->sectors = g->sect;
732 	geo->cylinders = g->track;
733 
734 	return 0;
735 }
736 
737 static unsigned int floppy_check_events(struct gendisk *disk,
738 					unsigned int clearing)
739 {
740 	struct floppy_state *fs = disk->private_data;
741 
742 	return fs->ejected ? DISK_EVENT_MEDIA_CHANGE : 0;
743 }
744 
745 static const struct block_device_operations floppy_fops = {
746 	.owner		 = THIS_MODULE,
747 	.open		 = floppy_unlocked_open,
748 	.release	 = floppy_release,
749 	.ioctl		 = floppy_ioctl,
750 	.getgeo		 = floppy_getgeo,
751 	.check_events	 = floppy_check_events,
752 };
753 
754 static void swim_add_floppy(struct swim_priv *swd, enum drive_location location)
755 {
756 	struct floppy_state *fs = &swd->unit[swd->floppy_count];
757 	struct swim __iomem *base = swd->base;
758 
759 	swim_drive(base, location);
760 	if (!swim_readbit(base, DRIVE_PRESENT) ||
761 	    swim_readbit(base, ONEMEG_DRIVE))
762 		goto out;
763 	if (swim_readbit(base, DISK_IN))
764 		swim_motor(base, ON);
765 	if (swim_track00(base))
766 		goto out;
767 
768 	fs->swd = swd;
769 	fs->location = location;
770 
771 	fs->type = HD_MEDIA;
772 	fs->head_number = 2;
773 
774 	fs->ref_count = 0;
775 	fs->ejected = 1;
776 	fs->track = 0;
777 
778 	swd->floppy_count++;
779 
780 out:
781 	swim_motor(base, OFF);
782 }
783 
784 static const struct blk_mq_ops swim_mq_ops = {
785 	.queue_rq = swim_queue_rq,
786 };
787 
788 static void swim_cleanup_floppy_disk(struct floppy_state *fs)
789 {
790 	struct gendisk *disk = fs->disk;
791 
792 	if (!disk)
793 		return;
794 
795 	if (fs->registered)
796 		del_gendisk(fs->disk);
797 
798 	put_disk(disk);
799 	blk_mq_free_tag_set(&fs->tag_set);
800 }
801 
802 static void swim_set_parameters(struct swim __iomem *base)
803 {
804 	unsigned int i;
805 	static const u8 mem[] = { 0x18, 0x41, 0x2e, 0x2e, 0x18, 0x18, 0x1b, 0x1b,
806 				  0x2f, 0x2f, 0x19, 0x19, 0x97, 0x1b, 0x57, 0x3b, };
807 
808 	swim_write(base, mode0, 0); /* reset parameter memory index */
809 	for (i = 0; i < 16; ++i)
810 		swim_write(base, parameter, mem[i]);
811 }
812 
813 static int swim_floppy_init(struct platform_device *pdev)
814 {
815 	struct swim_priv *swd = platform_get_drvdata(pdev);
816 	bool *data = pdev->dev.platform_data;
817 	bool fast_fclk = data && *data;
818 	struct queue_limits lim = {
819 		.features = BLK_FEAT_ROTATIONAL,
820 	};
821 	int err;
822 	int drive;
823 	struct swim __iomem *base = swd->base;
824 
825 	swim_write(base, setup, S_IBM_DRIVE | (fast_fclk ? S_FCLK_DIV2 : 0));
826 
827 	swim_set_parameters(base);
828 
829 	/* scan floppy drives */
830 
831 	swim_add_floppy(swd, INTERNAL_DRIVE);
832 	swim_add_floppy(swd, EXTERNAL_DRIVE);
833 	swim_drive(base, NO_DRIVE);
834 
835 	/* register floppy drives */
836 
837 	err = register_blkdev(FLOPPY_MAJOR, "fd");
838 	if (err) {
839 		printk(KERN_ERR "Unable to get major %d for SWIM floppy\n",
840 		       FLOPPY_MAJOR);
841 		return -EBUSY;
842 	}
843 
844 	for (drive = 0; drive < swd->floppy_count; drive++) {
845 		err = blk_mq_alloc_sq_tag_set(&swd->unit[drive].tag_set,
846 				&swim_mq_ops, 2, BLK_MQ_F_BLOCKING);
847 		if (err)
848 			goto exit_put_disks;
849 
850 		swd->unit[drive].disk =
851 			blk_mq_alloc_disk(&swd->unit[drive].tag_set, &lim,
852 					  &swd->unit[drive]);
853 		if (IS_ERR(swd->unit[drive].disk)) {
854 			blk_mq_free_tag_set(&swd->unit[drive].tag_set);
855 			err = PTR_ERR(swd->unit[drive].disk);
856 			goto exit_put_disks;
857 		}
858 	}
859 
860 	for (drive = 0; drive < swd->floppy_count; drive++) {
861 		swd->unit[drive].disk->flags = GENHD_FL_REMOVABLE;
862 		swd->unit[drive].disk->major = FLOPPY_MAJOR;
863 		swd->unit[drive].disk->first_minor = drive;
864 		swd->unit[drive].disk->minors = 1;
865 		sprintf(swd->unit[drive].disk->disk_name, "fd%d", drive);
866 		swd->unit[drive].disk->fops = &floppy_fops;
867 		swd->unit[drive].disk->flags |= GENHD_FL_NO_PART;
868 		swd->unit[drive].disk->events = DISK_EVENT_MEDIA_CHANGE;
869 		swd->unit[drive].disk->private_data = &swd->unit[drive];
870 		set_capacity(swd->unit[drive].disk, 2880);
871 		err = add_disk(swd->unit[drive].disk);
872 		if (err)
873 			goto exit_put_disks;
874 		swd->unit[drive].registered = true;
875 	}
876 
877 	return 0;
878 
879 exit_put_disks:
880 	unregister_blkdev(FLOPPY_MAJOR, "fd");
881 	do {
882 		swim_cleanup_floppy_disk(&swd->unit[drive]);
883 	} while (drive--);
884 	return err;
885 }
886 
887 static int swim_probe(struct platform_device *dev)
888 {
889 	struct resource *res;
890 	struct swim __iomem *swim_base;
891 	struct swim_priv *swd;
892 	int ret;
893 
894 	res = platform_get_resource(dev, IORESOURCE_MEM, 0);
895 	if (!res) {
896 		ret = -ENODEV;
897 		goto out;
898 	}
899 
900 	if (!request_mem_region(res->start, resource_size(res), CARDNAME)) {
901 		ret = -EBUSY;
902 		goto out;
903 	}
904 
905 	swim_base = (struct swim __iomem *)res->start;
906 	if (!swim_base) {
907 		ret = -ENOMEM;
908 		goto out_release_io;
909 	}
910 
911 	/* probe device */
912 
913 	set_swim_mode(swim_base, 1);
914 	if (!get_swim_mode(swim_base)) {
915 		printk(KERN_INFO "SWIM device not found !\n");
916 		ret = -ENODEV;
917 		goto out_release_io;
918 	}
919 
920 	/* set platform driver data */
921 
922 	swd = kzalloc_obj(struct swim_priv);
923 	if (!swd) {
924 		ret = -ENOMEM;
925 		goto out_release_io;
926 	}
927 	platform_set_drvdata(dev, swd);
928 
929 	swd->base = swim_base;
930 
931 	ret = swim_floppy_init(dev);
932 	if (ret)
933 		goto out_kfree;
934 
935 	return 0;
936 
937 out_kfree:
938 	kfree(swd);
939 out_release_io:
940 	release_mem_region(res->start, resource_size(res));
941 out:
942 	return ret;
943 }
944 
945 static void swim_remove(struct platform_device *dev)
946 {
947 	struct swim_priv *swd = platform_get_drvdata(dev);
948 	int drive;
949 	struct resource *res;
950 
951 	for (drive = 0; drive < swd->floppy_count; drive++)
952 		swim_cleanup_floppy_disk(&swd->unit[drive]);
953 
954 	unregister_blkdev(FLOPPY_MAJOR, "fd");
955 
956 	/* eject floppies */
957 
958 	for (drive = 0; drive < swd->floppy_count; drive++)
959 		floppy_eject(&swd->unit[drive]);
960 
961 	res = platform_get_resource(dev, IORESOURCE_MEM, 0);
962 	if (res)
963 		release_mem_region(res->start, resource_size(res));
964 
965 	kfree(swd);
966 }
967 
968 static struct platform_driver swim_driver = {
969 	.probe  = swim_probe,
970 	.remove = swim_remove,
971 	.driver = {
972 		.name	= CARDNAME,
973 	},
974 };
975 
976 static int __init swim_init(void)
977 {
978 	printk(KERN_INFO "SWIM floppy driver %s\n", DRIVER_VERSION);
979 
980 	return platform_driver_register(&swim_driver);
981 }
982 module_init(swim_init);
983 
984 static void __exit swim_exit(void)
985 {
986 	platform_driver_unregister(&swim_driver);
987 }
988 module_exit(swim_exit);
989 
990 MODULE_DESCRIPTION("Driver for SWIM floppy controller");
991 MODULE_LICENSE("GPL");
992 MODULE_AUTHOR("Laurent Vivier <laurent@lvivier.info>");
993 MODULE_ALIAS_BLOCKDEV_MAJOR(FLOPPY_MAJOR);
994