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