forked from ~ljy/RK356X_SDK_RELEASE

blame | history | patch | commit | commitdiff
hc
2024-02-20 102a0743326a03cd1a1202ceda21e175b7d3575c 
 kernel/drivers/block/ataflop.c
....@@ -1,3 +1,4 @@
1
+// SPDX-License-Identifier: GPL-2.0-only
12 /*
23  *  drivers/block/ataflop.c
34  *
....@@ -66,13 +67,11 @@
6667 #include <linux/fd.h>
6768 #include <linux/delay.h>
6869 #include <linux/init.h>
69
-#include <linux/blkdev.h>
70
+#include <linux/blk-mq.h>
7071 #include <linux/mutex.h>
7172 #include <linux/completion.h>
7273 #include <linux/wait.h>
7374 
74
-#include <asm/atafd.h>
75
-#include <asm/atafdreg.h>
7675 #include <asm/atariints.h>
7776 #include <asm/atari_stdma.h>
7877 #include <asm/atari_stram.h>
....@@ -83,7 +82,87 @@
8382 
8483 static DEFINE_MUTEX(ataflop_mutex);
8584 static struct request *fd_request;
86
-static int fdc_queue;
85
+
86
+/*
87
+ * WD1772 stuff
88
+ */
89
+
90
+/* register codes */
91
+
92
+#define FDCSELREG_STP   (0x80)   /* command/status register */
93
+#define FDCSELREG_TRA   (0x82)   /* track register */
94
+#define FDCSELREG_SEC   (0x84)   /* sector register */
95
+#define FDCSELREG_DTA   (0x86)   /* data register */
96
+
97
+/* register names for FDC_READ/WRITE macros */
98
+
99
+#define FDCREG_CMD		0
100
+#define FDCREG_STATUS	0
101
+#define FDCREG_TRACK	2
102
+#define FDCREG_SECTOR	4
103
+#define FDCREG_DATA		6
104
+
105
+/* command opcodes */
106
+
107
+#define FDCCMD_RESTORE  (0x00)   /*  -                   */
108
+#define FDCCMD_SEEK     (0x10)   /*   |                  */
109
+#define FDCCMD_STEP     (0x20)   /*   |  TYP 1 Commands  */
110
+#define FDCCMD_STIN     (0x40)   /*   |                  */
111
+#define FDCCMD_STOT     (0x60)   /*  -                   */
112
+#define FDCCMD_RDSEC    (0x80)   /*  -   TYP 2 Commands  */
113
+#define FDCCMD_WRSEC    (0xa0)   /*  -          "        */
114
+#define FDCCMD_RDADR    (0xc0)   /*  -                   */
115
+#define FDCCMD_RDTRA    (0xe0)   /*   |  TYP 3 Commands  */
116
+#define FDCCMD_WRTRA    (0xf0)   /*  -                   */
117
+#define FDCCMD_FORCI    (0xd0)   /*  -   TYP 4 Command   */
118
+
119
+/* command modifier bits */
120
+
121
+#define FDCCMDADD_SR6   (0x00)   /* step rate settings */
122
+#define FDCCMDADD_SR12  (0x01)
123
+#define FDCCMDADD_SR2   (0x02)
124
+#define FDCCMDADD_SR3   (0x03)
125
+#define FDCCMDADD_V     (0x04)   /* verify */
126
+#define FDCCMDADD_H     (0x08)   /* wait for spin-up */
127
+#define FDCCMDADD_U     (0x10)   /* update track register */
128
+#define FDCCMDADD_M     (0x10)   /* multiple sector access */
129
+#define FDCCMDADD_E     (0x04)   /* head settling flag */
130
+#define FDCCMDADD_P     (0x02)   /* precompensation off */
131
+#define FDCCMDADD_A0    (0x01)   /* DAM flag */
132
+
133
+/* status register bits */
134
+
135
+#define	FDCSTAT_MOTORON	(0x80)   /* motor on */
136
+#define	FDCSTAT_WPROT	(0x40)   /* write protected (FDCCMD_WR*) */
137
+#define	FDCSTAT_SPINUP	(0x20)   /* motor speed stable (Type I) */
138
+#define	FDCSTAT_DELDAM	(0x20)   /* sector has deleted DAM (Type II+III) */
139
+#define	FDCSTAT_RECNF	(0x10)   /* record not found */
140
+#define	FDCSTAT_CRC		(0x08)   /* CRC error */
141
+#define	FDCSTAT_TR00	(0x04)   /* Track 00 flag (Type I) */
142
+#define	FDCSTAT_LOST	(0x04)   /* Lost Data (Type II+III) */
143
+#define	FDCSTAT_IDX		(0x02)   /* Index status (Type I) */
144
+#define	FDCSTAT_DRQ		(0x02)   /* DRQ status (Type II+III) */
145
+#define	FDCSTAT_BUSY	(0x01)   /* FDC is busy */
146
+
147
+
148
+/* PSG Port A Bit Nr 0 .. Side Sel .. 0 -> Side 1  1 -> Side 2 */
149
+#define DSKSIDE     (0x01)
150
+
151
+#define DSKDRVNONE  (0x06)
152
+#define DSKDRV0     (0x02)
153
+#define DSKDRV1     (0x04)
154
+
155
+/* step rates */
156
+#define	FDCSTEP_6	0x00
157
+#define	FDCSTEP_12	0x01
158
+#define	FDCSTEP_2	0x02
159
+#define	FDCSTEP_3	0x03
160
+
161
+struct atari_format_descr {
162
+	int track;		/* to be formatted */
163
+	int head;		/*   ""     ""     */
164
+	int sect_offset;	/* offset of first sector */
165
+};
87166 
88167 /* Disk types: DD, HD, ED */
89168 static struct atari_disk_type {
....@@ -221,6 +300,7 @@
221300 	struct gendisk *disk;
222301 	int ref;
223302 	int type;
303
+	struct blk_mq_tag_set tag_set;
224304 } unit[FD_MAX_UNITS];
225305 
226306 #define	UD	unit[drive]
....@@ -300,9 +380,6 @@
300380 static int UserSteprate[FD_MAX_UNITS] = { -1, -1 };
301381 module_param_array(UserSteprate, int, NULL, 0);
302382 
303
-/* Synchronization of FDC access. */
304
-static volatile int fdc_busy = 0;
305
-static DECLARE_WAIT_QUEUE_HEAD(fdc_wait);
306383 static DECLARE_COMPLETION(format_wait);
307384 
308385 static unsigned long changed_floppies = 0xff, fake_change = 0;
....@@ -362,7 +439,6 @@
362439 static void finish_fdc( void );
363440 static void finish_fdc_done( int dummy );
364441 static void setup_req_params( int drive );
365
-static void redo_fd_request( void);
366442 static int fd_locked_ioctl(struct block_device *bdev, fmode_t mode, unsigned int
367443                      cmd, unsigned long param);
368444 static void fd_probe( int drive );
....@@ -380,8 +456,11 @@
380456 	
381457 static void fd_end_request_cur(blk_status_t err)
382458 {
383
-	if (!__blk_end_request_cur(fd_request, err))
459
+	if (!blk_update_request(fd_request, err,
460
+				blk_rq_cur_bytes(fd_request))) {
461
+		__blk_mq_end_request(fd_request, err);
384462 		fd_request = NULL;
463
+	}
385464 }
386465 
387466 static inline void start_motor_off_timer(void)
....@@ -627,7 +706,6 @@
627706 		if (SelectedDrive != -1)
628707 			SUD.track = -1;
629708 	}
630
-	redo_fd_request();
631709 }
632710 
633711 
....@@ -645,14 +723,15 @@
645723 
646724 static int do_format(int drive, int type, struct atari_format_descr *desc)
647725 {
726
+	struct request_queue *q = unit[drive].disk->queue;
648727 	unsigned char	*p;
649728 	int sect, nsect;
650729 	unsigned long	flags;
730
+	int ret;
651731 
652
-	DPRINT(("do_format( dr=%d tr=%d he=%d offs=%d )\n",
653
-		drive, desc->track, desc->head, desc->sect_offset ));
732
+	blk_mq_freeze_queue(q);
733
+	blk_mq_quiesce_queue(q);
654734 
655
-	wait_event(fdc_wait, cmpxchg(&fdc_busy, 0, 1) == 0);
656735 	local_irq_save(flags);
657736 	stdma_lock(floppy_irq, NULL);
658737 	atari_turnon_irq( IRQ_MFP_FDC ); /* should be already, just to be sure */
....@@ -661,16 +740,16 @@
661740 	if (type) {
662741 		if (--type >= NUM_DISK_MINORS ||
663742 		    minor2disktype[type].drive_types > DriveType) {
664
-			redo_fd_request();
665
-			return -EINVAL;
743
+			ret = -EINVAL;
744
+			goto out;
666745 		}
667746 		type = minor2disktype[type].index;
668747 		UDT = &atari_disk_type[type];
669748 	}
670749 
671750 	if (!UDT || desc->track >= UDT->blocks/UDT->spt/2 || desc->head >= 2) {
672
-		redo_fd_request();
673
-		return -EINVAL;
751
+		ret = -EINVAL;
752
+		goto out;
674753 	}
675754 
676755 	nsect = UDT->spt;
....@@ -709,8 +788,11 @@
709788 
710789 	wait_for_completion(&format_wait);
711790 
712
-	redo_fd_request();
713
-	return( FormatError ? -EIO : 0 );	
791
+	ret = FormatError ? -EIO : 0;
792
+out:
793
+	blk_mq_unquiesce_queue(q);
794
+	blk_mq_unfreeze_queue(q);
795
+	return ret;
714796 }
715797 
716798 
....@@ -740,7 +822,6 @@
740822 		    else {
741823 			/* all sectors finished */
742824 			fd_end_request_cur(BLK_STS_OK);
743
-			redo_fd_request();
744825 			return;
745826 		    }
746827 		}
....@@ -776,7 +857,7 @@
776857 	}
777858 
778859 	if (ATARIHW_PRESENT(FDCSPEED))
779
-		dma_wd.fdc_speed = 0; 	/* always seek with 8 Mhz */;
860
+		dma_wd.fdc_speed = 0;   /* always seek with 8 Mhz */
780861 	DPRINT(("fd_calibrate\n"));
781862 	SET_IRQ_HANDLER( fd_calibrate_done );
782863 	/* we can't verify, since the speed may be incorrect */
....@@ -1145,7 +1226,6 @@
11451226 	else {
11461227 		/* all sectors finished */
11471228 		fd_end_request_cur(BLK_STS_OK);
1148
-		redo_fd_request();
11491229 	}
11501230 	return;
11511231   
....@@ -1303,8 +1383,6 @@
13031383 
13041384 	local_irq_save(flags);
13051385 	stdma_release();
1306
-	fdc_busy = 0;
1307
-	wake_up( &fdc_wait );
13081386 	local_irq_restore(flags);
13091387 
13101388 	DPRINT(("finish_fdc() finished\n"));
....@@ -1394,59 +1472,43 @@
13941472 			ReqTrack, ReqSector, (unsigned long)ReqData ));
13951473 }
13961474 
1397
-/*
1398
- * Round-robin between our available drives, doing one request from each
1399
- */
1400
-static struct request *set_next_request(void)
1475
+static void ataflop_commit_rqs(struct blk_mq_hw_ctx *hctx)
14011476 {
1402
-	struct request_queue *q;
1403
-	int old_pos = fdc_queue;
1404
-	struct request *rq = NULL;
1405
-
1406
-	do {
1407
-		q = unit[fdc_queue].disk->queue;
1408
-		if (++fdc_queue == FD_MAX_UNITS)
1409
-			fdc_queue = 0;
1410
-		if (q) {
1411
-			rq = blk_fetch_request(q);
1412
-			if (rq) {
1413
-				rq->error_count = 0;
1414
-				break;
1415
-			}
1416
-		}
1417
-	} while (fdc_queue != old_pos);
1418
-
1419
-	return rq;
1477
+	spin_lock_irq(&ataflop_lock);
1478
+	atari_disable_irq(IRQ_MFP_FDC);
1479
+	finish_fdc();
1480
+	atari_enable_irq(IRQ_MFP_FDC);
1481
+	spin_unlock_irq(&ataflop_lock);
14201482 }
14211483 
1422
-
1423
-static void redo_fd_request(void)
1484
+static blk_status_t ataflop_queue_rq(struct blk_mq_hw_ctx *hctx,
1485
+				     const struct blk_mq_queue_data *bd)
14241486 {
1425
-	int drive, type;
1426
-	struct atari_floppy_struct *floppy;
1487
+	struct atari_floppy_struct *floppy = bd->rq->rq_disk->private_data;
1488
+	int drive = floppy - unit;
1489
+	int type = floppy->type;
14271490 
1428
-	DPRINT(("redo_fd_request: fd_request=%p dev=%s fd_request->sector=%ld\n",
1429
-		fd_request, fd_request ? fd_request->rq_disk->disk_name : "",
1430
-		fd_request ? blk_rq_pos(fd_request) : 0 ));
1491
+	spin_lock_irq(&ataflop_lock);
1492
+	if (fd_request) {
1493
+		spin_unlock_irq(&ataflop_lock);
1494
+		return BLK_STS_DEV_RESOURCE;
1495
+	}
1496
+	if (!stdma_try_lock(floppy_irq, NULL))  {
1497
+		spin_unlock_irq(&ataflop_lock);
1498
+		return BLK_STS_RESOURCE;
1499
+	}
1500
+	fd_request = bd->rq;
1501
+	blk_mq_start_request(fd_request);
1502
+
1503
+	atari_disable_irq( IRQ_MFP_FDC );
14311504 
14321505 	IsFormatting = 0;
14331506 
1434
-repeat:
1435
-	if (!fd_request) {
1436
-		fd_request = set_next_request();
1437
-		if (!fd_request)
1438
-			goto the_end;
1439
-	}
1440
-
1441
-	floppy = fd_request->rq_disk->private_data;
1442
-	drive = floppy - unit;
1443
-	type = floppy->type;
1444
-	
14451507 	if (!UD.connected) {
14461508 		/* drive not connected */
14471509 		printk(KERN_ERR "Unknown Device: fd%d\n", drive );
14481510 		fd_end_request_cur(BLK_STS_IOERR);
1449
-		goto repeat;
1511
+		goto out;
14501512 	}
14511513 		
14521514 	if (type == 0) {
....@@ -1462,22 +1524,17 @@
14621524 		if (--type >= NUM_DISK_MINORS) {
14631525 			printk(KERN_WARNING "fd%d: invalid disk format", drive );
14641526 			fd_end_request_cur(BLK_STS_IOERR);
1465
-			goto repeat;
1527
+			goto out;
14661528 		}
14671529 		if (minor2disktype[type].drive_types > DriveType)  {
14681530 			printk(KERN_WARNING "fd%d: unsupported disk format", drive );
14691531 			fd_end_request_cur(BLK_STS_IOERR);
1470
-			goto repeat;
1532
+			goto out;
14711533 		}
14721534 		type = minor2disktype[type].index;
14731535 		UDT = &atari_disk_type[type];
14741536 		set_capacity(floppy->disk, UDT->blocks);
14751537 		UD.autoprobe = 0;
1476
-	}
1477
-	
1478
-	if (blk_rq_pos(fd_request) + 1 > UDT->blocks) {
1479
-		fd_end_request_cur(BLK_STS_IOERR);
1480
-		goto repeat;
14811538 	}
14821539 
14831540 	/* stop deselect timer */
....@@ -1490,22 +1547,13 @@
14901547 	setup_req_params( drive );
14911548 	do_fd_action( drive );
14921549 
1493
-	return;
1494
-
1495
-  the_end:
1496
-	finish_fdc();
1497
-}
1498
-
1499
-
1500
-void do_fd_request(struct request_queue * q)
1501
-{
1502
-	DPRINT(("do_fd_request for pid %d\n",current->pid));
1503
-	wait_event(fdc_wait, cmpxchg(&fdc_busy, 0, 1) == 0);
1504
-	stdma_lock(floppy_irq, NULL);
1505
-
1506
-	atari_disable_irq( IRQ_MFP_FDC );
1507
-	redo_fd_request();
1550
+	if (bd->last)
1551
+		finish_fdc();
15081552 	atari_enable_irq( IRQ_MFP_FDC );
1553
+
1554
+out:
1555
+	spin_unlock_irq(&ataflop_lock);
1556
+	return BLK_STS_OK;
15091557 }
15101558 
15111559 static int fd_locked_ioctl(struct block_device *bdev, fmode_t mode,
....@@ -1583,7 +1631,6 @@
15831631 		/* what if type > 0 here? Overwrite specified entry ? */
15841632 		if (type) {
15851633 		        /* refuse to re-set a predefined type for now */
1586
-			redo_fd_request();
15871634 			return -EINVAL;
15881635 		}
15891636 
....@@ -1651,10 +1698,8 @@
16511698 
16521699 		/* sanity check */
16531700 		if (setprm.track != dtp->blocks/dtp->spt/2 ||
1654
-		    setprm.head != 2) {
1655
-			redo_fd_request();
1701
+		    setprm.head != 2)
16561702 			return -EINVAL;
1657
-		}
16581703 
16591704 		UDT = dtp;
16601705 		set_capacity(floppy->disk, UDT->blocks);
....@@ -1681,12 +1726,14 @@
16811726 		/* MSch: invalidate default_params */
16821727 		default_params[drive].blocks  = 0;
16831728 		set_capacity(floppy->disk, MAX_DISK_SIZE * 2);
1729
+		fallthrough;
16841730 	case FDFMTEND:
16851731 	case FDFLUSH:
16861732 		/* invalidate the buffer track to force a reread */
16871733 		BufferDrive = -1;
16881734 		set_bit(drive, &fake_change);
1689
-		check_disk_change(bdev);
1735
+		if (bdev_check_media_change(bdev))
1736
+			floppy_revalidate(bdev->bd_disk);
16901737 		return 0;
16911738 	default:
16921739 		return -EINVAL;
....@@ -1863,7 +1910,8 @@
18631910 		return 0;
18641911 
18651912 	if (mode & (FMODE_READ|FMODE_WRITE)) {
1866
-		check_disk_change(bdev);
1913
+		if (bdev_check_media_change(bdev))
1914
+			floppy_revalidate(bdev->bd_disk);
18671915 		if (mode & FMODE_WRITE) {
18681916 			if (p->wpstat) {
18691917 				if (p->ref < 0)
....@@ -1907,7 +1955,11 @@
19071955 	.release	= floppy_release,
19081956 	.ioctl		= fd_ioctl,
19091957 	.check_events	= floppy_check_events,
1910
-	.revalidate_disk= floppy_revalidate,
1958
+};
1959
+
1960
+static const struct blk_mq_ops ataflop_mq_ops = {
1961
+	.queue_rq = ataflop_queue_rq,
1962
+	.commit_rqs = ataflop_commit_rqs,
19111963 };
19121964 
19131965 static struct kobject *floppy_find(dev_t dev, int *part, void *data)
....@@ -1923,6 +1975,7 @@
19231975 static int __init atari_floppy_init (void)
19241976 {
19251977 	int i;
1978
+	int ret;
19261979 
19271980 	if (!MACH_IS_ATARI)
19281981 		/* Amiga, Mac, ... don't have Atari-compatible floppy :-) */
....@@ -1933,13 +1986,20 @@
19331986 
19341987 	for (i = 0; i < FD_MAX_UNITS; i++) {
19351988 		unit[i].disk = alloc_disk(1);
1936
-		if (!unit[i].disk)
1937
-			goto Enomem;
1989
+		if (!unit[i].disk) {
1990
+			ret = -ENOMEM;
1991
+			goto err;
1992
+		}
19381993 
1939
-		unit[i].disk->queue = blk_init_queue(do_fd_request,
1940
-						     &ataflop_lock);
1941
-		if (!unit[i].disk->queue)
1942
-			goto Enomem;
1994
+		unit[i].disk->queue = blk_mq_init_sq_queue(&unit[i].tag_set,
1995
+							   &ataflop_mq_ops, 2,
1996
+							   BLK_MQ_F_SHOULD_MERGE);
1997
+		if (IS_ERR(unit[i].disk->queue)) {
1998
+			put_disk(unit[i].disk);
1999
+			ret = PTR_ERR(unit[i].disk->queue);
2000
+			unit[i].disk->queue = NULL;
2001
+			goto err;
2002
+		}
19432003 	}
19442004 
19452005 	if (UseTrackbuffer < 0)
....@@ -1956,7 +2016,8 @@
19562016 	DMABuffer = atari_stram_alloc(BUFFER_SIZE+512, "ataflop");
19572017 	if (!DMABuffer) {
19582018 		printk(KERN_ERR "atari_floppy_init: cannot get dma buffer\n");
1959
-		goto Enomem;
2019
+		ret = -ENOMEM;
2020
+		goto err;
19602021 	}
19612022 	TrackBuffer = DMABuffer + 512;
19622023 	PhysDMABuffer = atari_stram_to_phys(DMABuffer);
....@@ -1970,6 +2031,7 @@
19702031 		unit[i].disk->first_minor = i;
19712032 		sprintf(unit[i].disk->disk_name, "fd%d", i);
19722033 		unit[i].disk->fops = &floppy_fops;
2034
+		unit[i].disk->events = DISK_EVENT_MEDIA_CHANGE;
19732035 		unit[i].disk->private_data = &unit[i];
19742036 		set_capacity(unit[i].disk, MAX_DISK_SIZE * 2);
19752037 		add_disk(unit[i].disk);
....@@ -1984,21 +2046,18 @@
19842046 	config_types();
19852047 
19862048 	return 0;
1987
-Enomem:
1988
-	do {
2049
+
2050
+err:
2051
+	while (--i >= 0) {
19892052 		struct gendisk *disk = unit[i].disk;
19902053 
1991
-		if (disk) {
1992
-			if (disk->queue) {
1993
-				blk_cleanup_queue(disk->queue);
1994
-				disk->queue = NULL;
1995
-			}
1996
-			put_disk(unit[i].disk);
1997
-		}
1998
-	} while (i--);
2054
+		blk_cleanup_queue(disk->queue);
2055
+		blk_mq_free_tag_set(&unit[i].tag_set);
2056
+		put_disk(unit[i].disk);
2057
+	}
19992058 
20002059 	unregister_blkdev(FLOPPY_MAJOR, "fd");
2001
-	return -ENOMEM;
2060
+	return ret;
20022061 }
20032062 
20042063 #ifndef MODULE
....@@ -2045,11 +2104,10 @@
20452104 	int i;
20462105 	blk_unregister_region(MKDEV(FLOPPY_MAJOR, 0), 256);
20472106 	for (i = 0; i < FD_MAX_UNITS; i++) {
2048
-		struct request_queue *q = unit[i].disk->queue;
2049
-
20502107 		del_gendisk(unit[i].disk);
2108
+		blk_cleanup_queue(unit[i].disk->queue);
2109
+		blk_mq_free_tag_set(&unit[i].tag_set);
20512110 		put_disk(unit[i].disk);
2052
-		blk_cleanup_queue(q);
20532111 	}
20542112 	unregister_blkdev(FLOPPY_MAJOR, "fd");
20552113