forked from ~ljy/RK356X_SDK_RELEASE

blame | history | patch | commit | commitdiff
hc
2024-05-10 9999e48639b3cecb08ffb37358bcba3b48161b29 
 kernel/drivers/block/swim3.c
....@@ -1,13 +1,9 @@
1
+// SPDX-License-Identifier: GPL-2.0-or-later
12 /*
23  * Driver for the SWIM3 (Super Woz Integrated Machine 3)
34  * floppy controller found on Power Macintoshes.
45  *
56  * Copyright (C) 1996 Paul Mackerras.
6
- *
7
- * This program is free software; you can redistribute it and/or
8
- * modify it under the terms of the GNU General Public License
9
- * as published by the Free Software Foundation; either version
10
- * 2 of the License, or (at your option) any later version.
117  */
128 
139 /*
....@@ -25,7 +21,7 @@
2521 #include <linux/delay.h>
2622 #include <linux/fd.h>
2723 #include <linux/ioctl.h>
28
-#include <linux/blkdev.h>
24
+#include <linux/blk-mq.h>
2925 #include <linux/interrupt.h>
3026 #include <linux/mutex.h>
3127 #include <linux/module.h>
....@@ -206,6 +202,7 @@
206202 	char	dbdma_cmd_space[5 * sizeof(struct dbdma_cmd)];
207203 	int	index;
208204 	struct request *cur_req;
205
+	struct blk_mq_tag_set tag_set;
209206 };
210207 
211208 #define swim3_err(fmt, arg...)	dev_err(&fs->mdev->ofdev.dev, "[fd%d] " fmt, fs->index, arg)
....@@ -260,16 +257,15 @@
260257 static bool swim3_end_request(struct floppy_state *fs, blk_status_t err, unsigned int nr_bytes)
261258 {
262259 	struct request *req = fs->cur_req;
263
-	int rc;
264260 
265261 	swim3_dbg("  end request, err=%d nr_bytes=%d, cur_req=%p\n",
266262 		  err, nr_bytes, req);
267263 
268264 	if (err)
269265 		nr_bytes = blk_rq_cur_bytes(req);
270
-	rc = __blk_end_request(req, err, nr_bytes);
271
-	if (rc)
266
+	if (blk_update_request(req, err, nr_bytes))
272267 		return true;
268
+	__blk_mq_end_request(req, err);
273269 	fs->cur_req = NULL;
274270 	return false;
275271 }
....@@ -309,86 +305,58 @@
309305 	return (stat & DATA) == 0;
310306 }
311307 
312
-static void start_request(struct floppy_state *fs)
308
+static blk_status_t swim3_queue_rq(struct blk_mq_hw_ctx *hctx,
309
+				   const struct blk_mq_queue_data *bd)
313310 {
314
-	struct request *req;
311
+	struct floppy_state *fs = hctx->queue->queuedata;
312
+	struct request *req = bd->rq;
315313 	unsigned long x;
316314 
317
-	swim3_dbg("start request, initial state=%d\n", fs->state);
318
-
319
-	if (fs->state == idle && fs->wanted) {
320
-		fs->state = available;
321
-		wake_up(&fs->wait);
322
-		return;
315
+	spin_lock_irq(&swim3_lock);
316
+	if (fs->cur_req || fs->state != idle) {
317
+		spin_unlock_irq(&swim3_lock);
318
+		return BLK_STS_DEV_RESOURCE;
323319 	}
324
-	while (fs->state == idle) {
325
-		swim3_dbg("start request, idle loop, cur_req=%p\n", fs->cur_req);
326
-		if (!fs->cur_req) {
327
-			fs->cur_req = blk_fetch_request(disks[fs->index]->queue);
328
-			swim3_dbg("  fetched request %p\n", fs->cur_req);
329
-			if (!fs->cur_req)
330
-				break;
331
-		}
332
-		req = fs->cur_req;
333
-
334
-		if (fs->mdev->media_bay &&
335
-		    check_media_bay(fs->mdev->media_bay) != MB_FD) {
336
-			swim3_dbg("%s", "  media bay absent, dropping req\n");
337
-			swim3_end_request(fs, BLK_STS_IOERR, 0);
338
-			continue;
339
-		}
340
-
341
-#if 0 /* This is really too verbose */
342
-		swim3_dbg("do_fd_req: dev=%s cmd=%d sec=%ld nr_sec=%u buf=%p\n",
343
-			  req->rq_disk->disk_name, req->cmd,
344
-			  (long)blk_rq_pos(req), blk_rq_sectors(req),
345
-			  bio_data(req->bio));
346
-		swim3_dbg("           current_nr_sectors=%u\n",
347
-			  blk_rq_cur_sectors(req));
348
-#endif
349
-
350
-		if (blk_rq_pos(req) >= fs->total_secs) {
351
-			swim3_dbg("  pos out of bounds (%ld, max is %ld)\n",
352
-				  (long)blk_rq_pos(req), (long)fs->total_secs);
353
-			swim3_end_request(fs, BLK_STS_IOERR, 0);
354
-			continue;
355
-		}
356
-		if (fs->ejected) {
357
-			swim3_dbg("%s", "  disk ejected\n");
358
-			swim3_end_request(fs, BLK_STS_IOERR, 0);
359
-			continue;
360
-		}
361
-
362
-		if (rq_data_dir(req) == WRITE) {
363
-			if (fs->write_prot < 0)
364
-				fs->write_prot = swim3_readbit(fs, WRITE_PROT);
365
-			if (fs->write_prot) {
366
-				swim3_dbg("%s", "  try to write, disk write protected\n");
367
-				swim3_end_request(fs, BLK_STS_IOERR, 0);
368
-				continue;
369
-			}
370
-		}
371
-
372
-		/* Do not remove the cast. blk_rq_pos(req) is now a
373
-		 * sector_t and can be 64 bits, but it will never go
374
-		 * past 32 bits for this driver anyway, so we can
375
-		 * safely cast it down and not have to do a 64/32
376
-		 * division
377
-		 */
378
-		fs->req_cyl = ((long)blk_rq_pos(req)) / fs->secpercyl;
379
-		x = ((long)blk_rq_pos(req)) % fs->secpercyl;
380
-		fs->head = x / fs->secpertrack;
381
-		fs->req_sector = x % fs->secpertrack + 1;
382
-		fs->state = do_transfer;
383
-		fs->retries = 0;
384
-
385
-		act(fs);
320
+	blk_mq_start_request(req);
321
+	fs->cur_req = req;
322
+	if (fs->mdev->media_bay &&
323
+	    check_media_bay(fs->mdev->media_bay) != MB_FD) {
324
+		swim3_dbg("%s", "  media bay absent, dropping req\n");
325
+		swim3_end_request(fs, BLK_STS_IOERR, 0);
326
+		goto out;
386327 	}
387
-}
328
+	if (fs->ejected) {
329
+		swim3_dbg("%s", "  disk ejected\n");
330
+		swim3_end_request(fs, BLK_STS_IOERR, 0);
331
+		goto out;
332
+	}
333
+	if (rq_data_dir(req) == WRITE) {
334
+		if (fs->write_prot < 0)
335
+			fs->write_prot = swim3_readbit(fs, WRITE_PROT);
336
+		if (fs->write_prot) {
337
+			swim3_dbg("%s", "  try to write, disk write protected\n");
338
+			swim3_end_request(fs, BLK_STS_IOERR, 0);
339
+			goto out;
340
+		}
341
+	}
388342 
389
-static void do_fd_request(struct request_queue * q)
390
-{
391
-	start_request(q->queuedata);
343
+	/*
344
+	 * Do not remove the cast. blk_rq_pos(req) is now a sector_t and can be
345
+	 * 64 bits, but it will never go past 32 bits for this driver anyway, so
346
+	 * we can safely cast it down and not have to do a 64/32 division
347
+	 */
348
+	fs->req_cyl = ((long)blk_rq_pos(req)) / fs->secpercyl;
349
+	x = ((long)blk_rq_pos(req)) % fs->secpercyl;
350
+	fs->head = x / fs->secpertrack;
351
+	fs->req_sector = x % fs->secpertrack + 1;
352
+	fs->state = do_transfer;
353
+	fs->retries = 0;
354
+
355
+	act(fs);
356
+
357
+out:
358
+	spin_unlock_irq(&swim3_lock);
359
+	return BLK_STS_OK;
392360 }
393361 
394362 static void set_timeout(struct floppy_state *fs, int nticks,
....@@ -585,7 +553,6 @@
585553 	if (fs->retries > 5) {
586554 		swim3_end_request(fs, BLK_STS_IOERR, 0);
587555 		fs->state = idle;
588
-		start_request(fs);
589556 	} else {
590557 		fs->state = jogging;
591558 		act(fs);
....@@ -609,7 +576,6 @@
609576 	swim3_err("%s", "Seek timeout\n");
610577 	swim3_end_request(fs, BLK_STS_IOERR, 0);
611578 	fs->state = idle;
612
-	start_request(fs);
613579 	spin_unlock_irqrestore(&swim3_lock, flags);
614580 }
615581 
....@@ -638,7 +604,6 @@
638604 	swim3_err("%s", "Seek settle timeout\n");
639605 	swim3_end_request(fs, BLK_STS_IOERR, 0);
640606 	fs->state = idle;
641
-	start_request(fs);
642607  unlock:
643608 	spin_unlock_irqrestore(&swim3_lock, flags);
644609 }
....@@ -667,7 +632,6 @@
667632 	       (long)blk_rq_pos(fs->cur_req));
668633 	swim3_end_request(fs, BLK_STS_IOERR, 0);
669634 	fs->state = idle;
670
-	start_request(fs);
671635 	spin_unlock_irqrestore(&swim3_lock, flags);
672636 }
673637 
....@@ -704,7 +668,6 @@
704668 				if (fs->retries > 5) {
705669 					swim3_end_request(fs, BLK_STS_IOERR, 0);
706670 					fs->state = idle;
707
-					start_request(fs);
708671 				} else {
709672 					fs->state = jogging;
710673 					act(fs);
....@@ -796,7 +759,6 @@
796759 					  fs->state, rq_data_dir(req), intr, err);
797760 				swim3_end_request(fs, BLK_STS_IOERR, 0);
798761 				fs->state = idle;
799
-				start_request(fs);
800762 				break;
801763 			}
802764 			fs->retries = 0;
....@@ -813,8 +775,6 @@
813775 			} else
814776 				fs->state = idle;
815777 		}
816
-		if (fs->state == idle)
817
-			start_request(fs);
818778 		break;
819779 	default:
820780 		swim3_err("Don't know what to do in state %d\n", fs->state);
....@@ -862,14 +822,19 @@
862822 
863823 static void release_drive(struct floppy_state *fs)
864824 {
825
+	struct request_queue *q = disks[fs->index]->queue;
865826 	unsigned long flags;
866827 
867828 	swim3_dbg("%s", "-> release drive\n");
868829 
869830 	spin_lock_irqsave(&swim3_lock, flags);
870831 	fs->state = idle;
871
-	start_request(fs);
872832 	spin_unlock_irqrestore(&swim3_lock, flags);
833
+
834
+	blk_mq_freeze_queue(q);
835
+	blk_mq_quiesce_queue(q);
836
+	blk_mq_unquiesce_queue(q);
837
+	blk_mq_unfreeze_queue(q);
873838 }
874839 
875840 static int fd_eject(struct floppy_state *fs)
....@@ -980,7 +945,8 @@
980945 
981946 	if (err == 0 && (mode & FMODE_NDELAY) == 0
982947 	    && (mode & (FMODE_READ|FMODE_WRITE))) {
983
-		check_disk_change(bdev);
948
+		if (bdev_check_media_change(bdev))
949
+			floppy_revalidate(bdev->bd_disk);
984950 		if (fs->ejected)
985951 			err = -ENXIO;
986952 	}
....@@ -1090,7 +1056,10 @@
10901056 	.release	= floppy_release,
10911057 	.ioctl		= floppy_ioctl,
10921058 	.check_events	= floppy_check_events,
1093
-	.revalidate_disk= floppy_revalidate,
1059
+};
1060
+
1061
+static const struct blk_mq_ops swim3_mq_ops = {
1062
+	.queue_rq = swim3_queue_rq,
10941063 };
10951064 
10961065 static void swim3_mb_event(struct macio_dev* mdev, int mb_state)
....@@ -1118,8 +1087,6 @@
11181087 	struct floppy_state *fs = &floppy_states[index];
11191088 	int rc = -EBUSY;
11201089 
1121
-	/* Do this first for message macros */
1122
-	memset(fs, 0, sizeof(*fs));
11231090 	fs->mdev = mdev;
11241091 	fs->index = index;
11251092 
....@@ -1182,7 +1149,6 @@
11821149 		swim3_err("%s", "Couldn't request interrupt\n");
11831150 		pmac_call_feature(PMAC_FTR_SWIM3_ENABLE, swim, 0, 0);
11841151 		goto out_unmap;
1185
-		return -EBUSY;
11861152 	}
11871153 
11881154 	timer_setup(&fs->timeout, NULL, 0);
....@@ -1206,47 +1172,65 @@
12061172 static int swim3_attach(struct macio_dev *mdev,
12071173 			const struct of_device_id *match)
12081174 {
1175
+	struct floppy_state *fs;
12091176 	struct gendisk *disk;
1210
-	int index, rc;
1177
+	int rc;
12111178 
1212
-	index = floppy_count++;
1213
-	if (index >= MAX_FLOPPIES)
1179
+	if (floppy_count >= MAX_FLOPPIES)
12141180 		return -ENXIO;
12151181 
1216
-	/* Add the drive */
1217
-	rc = swim3_add_device(mdev, index);
1218
-	if (rc)
1219
-		return rc;
1220
-	/* Now register that disk. Same comment about failure handling */
1221
-	disk = disks[index] = alloc_disk(1);
1222
-	if (disk == NULL)
1223
-		return -ENOMEM;
1224
-	disk->queue = blk_init_queue(do_fd_request, &swim3_lock);
1225
-	if (disk->queue == NULL) {
1226
-		put_disk(disk);
1227
-		return -ENOMEM;
1182
+	if (floppy_count == 0) {
1183
+		rc = register_blkdev(FLOPPY_MAJOR, "fd");
1184
+		if (rc)
1185
+			return rc;
1186
+	}
1187
+
1188
+	disk = alloc_disk(1);
1189
+	if (disk == NULL) {
1190
+		rc = -ENOMEM;
1191
+		goto out_unregister;
1192
+	}
1193
+
1194
+	fs = &floppy_states[floppy_count];
1195
+	memset(fs, 0, sizeof(*fs));
1196
+
1197
+	disk->queue = blk_mq_init_sq_queue(&fs->tag_set, &swim3_mq_ops, 2,
1198
+						BLK_MQ_F_SHOULD_MERGE);
1199
+	if (IS_ERR(disk->queue)) {
1200
+		rc = PTR_ERR(disk->queue);
1201
+		disk->queue = NULL;
1202
+		goto out_put_disk;
12281203 	}
12291204 	blk_queue_bounce_limit(disk->queue, BLK_BOUNCE_HIGH);
1230
-	disk->queue->queuedata = &floppy_states[index];
1205
+	disk->queue->queuedata = fs;
12311206 
1232
-	if (index == 0) {
1233
-		/* If we failed, there isn't much we can do as the driver is still
1234
-		 * too dumb to remove the device, just bail out
1235
-		 */
1236
-		if (register_blkdev(FLOPPY_MAJOR, "fd"))
1237
-			return 0;
1238
-	}
1207
+	rc = swim3_add_device(mdev, floppy_count);
1208
+	if (rc)
1209
+		goto out_cleanup_queue;
12391210 
12401211 	disk->major = FLOPPY_MAJOR;
1241
-	disk->first_minor = index;
1212
+	disk->first_minor = floppy_count;
12421213 	disk->fops = &floppy_fops;
1243
-	disk->private_data = &floppy_states[index];
1214
+	disk->private_data = fs;
1215
+	disk->events = DISK_EVENT_MEDIA_CHANGE;
12441216 	disk->flags |= GENHD_FL_REMOVABLE;
1245
-	sprintf(disk->disk_name, "fd%d", index);
1217
+	sprintf(disk->disk_name, "fd%d", floppy_count);
12461218 	set_capacity(disk, 2880);
12471219 	add_disk(disk);
12481220 
1221
+	disks[floppy_count++] = disk;
12491222 	return 0;
1223
+
1224
+out_cleanup_queue:
1225
+	blk_cleanup_queue(disk->queue);
1226
+	disk->queue = NULL;
1227
+	blk_mq_free_tag_set(&fs->tag_set);
1228
+out_put_disk:
1229
+	put_disk(disk);
1230
+out_unregister:
1231
+	if (floppy_count == 0)
1232
+		unregister_blkdev(FLOPPY_MAJOR, "fd");
1233
+	return rc;
12501234 }
12511235 
12521236 static const struct of_device_id swim3_match[] =