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