rbd: warn on update_snaps failure on notify
[linux-2.6.git] / drivers / block / swim.c
1 /*
2  * Driver for SWIM (Sander Woz Integrated Machine) floppy controller
3  *
4  * Copyright (C) 2004,2008 Laurent Vivier <Laurent@lvivier.info>
5  *
6  * based on Alastair Bridgewater SWIM analysis, 2001
7  * based on SWIM3 driver (c) Paul Mackerras, 1996
8  * based on netBSD IWM driver (c) 1997, 1998 Hauke Fath.
9  *
10  * This program is free software; you can redistribute it and/or
11  * modify it under the terms of the GNU General Public License
12  * as published by the Free Software Foundation; either version
13  * 2 of the License, or (at your option) any later version.
14  *
15  * 2004-08-21 (lv) - Initial implementation
16  * 2008-10-30 (lv) - Port to 2.6
17  */
18
19 #include <linux/module.h>
20 #include <linux/fd.h>
21 #include <linux/slab.h>
22 #include <linux/blkdev.h>
23 #include <linux/mutex.h>
24 #include <linux/hdreg.h>
25 #include <linux/kernel.h>
26 #include <linux/delay.h>
27 #include <linux/platform_device.h>
28
29 #include <asm/macintosh.h>
30 #include <asm/mac_via.h>
31
32 #define CARDNAME "swim"
33
34 struct sector_header {
35         unsigned char side;
36         unsigned char track;
37         unsigned char sector;
38         unsigned char size;
39         unsigned char crc0;
40         unsigned char crc1;
41 } __attribute__((packed));
42
43 #define DRIVER_VERSION "Version 0.2 (2008-10-30)"
44
45 #define REG(x)  unsigned char x, x ## _pad[0x200 - 1];
46
47 struct swim {
48         REG(write_data)
49         REG(write_mark)
50         REG(write_CRC)
51         REG(write_parameter)
52         REG(write_phase)
53         REG(write_setup)
54         REG(write_mode0)
55         REG(write_mode1)
56
57         REG(read_data)
58         REG(read_mark)
59         REG(read_error)
60         REG(read_parameter)
61         REG(read_phase)
62         REG(read_setup)
63         REG(read_status)
64         REG(read_handshake)
65 } __attribute__((packed));
66
67 #define swim_write(base, reg, v)        out_8(&(base)->write_##reg, (v))
68 #define swim_read(base, reg)            in_8(&(base)->read_##reg)
69
70 /* IWM registers */
71
72 struct iwm {
73         REG(ph0L)
74         REG(ph0H)
75         REG(ph1L)
76         REG(ph1H)
77         REG(ph2L)
78         REG(ph2H)
79         REG(ph3L)
80         REG(ph3H)
81         REG(mtrOff)
82         REG(mtrOn)
83         REG(intDrive)
84         REG(extDrive)
85         REG(q6L)
86         REG(q6H)
87         REG(q7L)
88         REG(q7H)
89 } __attribute__((packed));
90
91 #define iwm_write(base, reg, v)         out_8(&(base)->reg, (v))
92 #define iwm_read(base, reg)             in_8(&(base)->reg)
93
94 /* bits in phase register */
95
96 #define SEEK_POSITIVE   0x070
97 #define SEEK_NEGATIVE   0x074
98 #define STEP            0x071
99 #define MOTOR_ON        0x072
100 #define MOTOR_OFF       0x076
101 #define INDEX           0x073
102 #define EJECT           0x077
103 #define SETMFM          0x171
104 #define SETGCR          0x175
105
106 #define RELAX           0x033
107 #define LSTRB           0x008
108
109 #define CA_MASK         0x077
110
111 /* Select values for swim_select and swim_readbit */
112
113 #define READ_DATA_0     0x074
114 #define TWOMEG_DRIVE    0x075
115 #define SINGLE_SIDED    0x076
116 #define DRIVE_PRESENT   0x077
117 #define DISK_IN         0x170
118 #define WRITE_PROT      0x171
119 #define TRACK_ZERO      0x172
120 #define TACHO           0x173
121 #define READ_DATA_1     0x174
122 #define MFM_MODE        0x175
123 #define SEEK_COMPLETE   0x176
124 #define ONEMEG_MEDIA    0x177
125
126 /* Bits in handshake register */
127
128 #define MARK_BYTE       0x01
129 #define CRC_ZERO        0x02
130 #define RDDATA          0x04
131 #define SENSE           0x08
132 #define MOTEN           0x10
133 #define ERROR           0x20
134 #define DAT2BYTE        0x40
135 #define DAT1BYTE        0x80
136
137 /* bits in setup register */
138
139 #define S_INV_WDATA     0x01
140 #define S_3_5_SELECT    0x02
141 #define S_GCR           0x04
142 #define S_FCLK_DIV2     0x08
143 #define S_ERROR_CORR    0x10
144 #define S_IBM_DRIVE     0x20
145 #define S_GCR_WRITE     0x40
146 #define S_TIMEOUT       0x80
147
148 /* bits in mode register */
149
150 #define CLFIFO          0x01
151 #define ENBL1           0x02
152 #define ENBL2           0x04
153 #define ACTION          0x08
154 #define WRITE_MODE      0x10
155 #define HEDSEL          0x20
156 #define MOTON           0x80
157
158 /*----------------------------------------------------------------------------*/
159
160 enum drive_location {
161         INTERNAL_DRIVE = 0x02,
162         EXTERNAL_DRIVE = 0x04,
163 };
164
165 enum media_type {
166         DD_MEDIA,
167         HD_MEDIA,
168 };
169
170 struct floppy_state {
171
172         /* physical properties */
173
174         enum drive_location location;   /* internal or external drive */
175         int              head_number;   /* single- or double-sided drive */
176
177         /* media */
178
179         int              disk_in;
180         int              ejected;
181         enum media_type  type;
182         int              write_protected;
183
184         int              total_secs;
185         int              secpercyl;
186         int              secpertrack;
187
188         /* in-use information */
189
190         int             track;
191         int             ref_count;
192
193         struct gendisk *disk;
194
195         /* parent controller */
196
197         struct swim_priv *swd;
198 };
199
200 enum motor_action {
201         OFF,
202         ON,
203 };
204
205 enum head {
206         LOWER_HEAD = 0,
207         UPPER_HEAD = 1,
208 };
209
210 #define FD_MAX_UNIT     2
211
212 struct swim_priv {
213         struct swim __iomem *base;
214         spinlock_t lock;
215         struct request_queue *queue;
216         int floppy_count;
217         struct floppy_state unit[FD_MAX_UNIT];
218 };
219
220 extern int swim_read_sector_header(struct swim __iomem *base,
221                                    struct sector_header *header);
222 extern int swim_read_sector_data(struct swim __iomem *base,
223                                  unsigned char *data);
224
225 static DEFINE_MUTEX(swim_mutex);
226 static inline void set_swim_mode(struct swim __iomem *base, int enable)
227 {
228         struct iwm __iomem *iwm_base;
229         unsigned long flags;
230
231         if (!enable) {
232                 swim_write(base, mode0, 0xf8);
233                 return;
234         }
235
236         iwm_base = (struct iwm __iomem *)base;
237         local_irq_save(flags);
238
239         iwm_read(iwm_base, q7L);
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 inline 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);
276
277         via1_set_head(sel & 0x100);
278
279         swim_write(base, phase, sel & CA_MASK);
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<<4) | LSTRB);
291         udelay(1);
292         swim_write(base, phase, (LSTRB<<4) | ((~LSTRB) & 0x0F));
293         udelay(1);
294
295         local_irq_restore(flags);
296 }
297
298 static inline int 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 static inline void swim_drive(struct swim __iomem *base,
312                               enum drive_location location)
313 {
314         if (location == INTERNAL_DRIVE) {
315                 swim_write(base, mode0, EXTERNAL_DRIVE); /* clear drive 1 bit */
316                 swim_write(base, mode1, INTERNAL_DRIVE); /* set drive 0 bit */
317         } else if (location == EXTERNAL_DRIVE) {
318                 swim_write(base, mode0, INTERNAL_DRIVE); /* clear drive 0 bit */
319                 swim_write(base, mode1, EXTERNAL_DRIVE); /* set drive 1 bit */
320         }
321 }
322
323 static inline void swim_motor(struct swim __iomem *base,
324                               enum motor_action action)
325 {
326         if (action == ON) {
327                 int i;
328
329                 swim_action(base, MOTOR_ON);
330
331                 for (i = 0; i < 2*HZ; i++) {
332                         swim_select(base, RELAX);
333                         if (swim_readbit(base, MOTOR_ON))
334                                 break;
335                         current->state = TASK_INTERRUPTIBLE;
336                         schedule_timeout(1);
337                 }
338         } else if (action == OFF) {
339                 swim_action(base, MOTOR_OFF);
340                 swim_select(base, RELAX);
341         }
342 }
343
344 static inline void swim_eject(struct swim __iomem *base)
345 {
346         int i;
347
348         swim_action(base, EJECT);
349
350         for (i = 0; i < 2*HZ; i++) {
351                 swim_select(base, RELAX);
352                 if (!swim_readbit(base, DISK_IN))
353                         break;
354                 current->state = TASK_INTERRUPTIBLE;
355                 schedule_timeout(1);
356         }
357         swim_select(base, RELAX);
358 }
359
360 static inline void swim_head(struct swim __iomem *base, enum head head)
361 {
362         /* wait drive is ready */
363
364         if (head == UPPER_HEAD)
365                 swim_select(base, READ_DATA_1);
366         else if (head == LOWER_HEAD)
367                 swim_select(base, READ_DATA_0);
368 }
369
370 static inline int swim_step(struct swim __iomem *base)
371 {
372         int wait;
373
374         swim_action(base, STEP);
375
376         for (wait = 0; wait < HZ; wait++) {
377
378                 current->state = TASK_INTERRUPTIBLE;
379                 schedule_timeout(1);
380
381                 swim_select(base, RELAX);
382                 if (!swim_readbit(base, STEP))
383                         return 0;
384         }
385         return -1;
386 }
387
388 static inline int swim_track00(struct swim __iomem *base)
389 {
390         int try;
391
392         swim_action(base, SEEK_NEGATIVE);
393
394         for (try = 0; try < 100; try++) {
395
396                 swim_select(base, RELAX);
397                 if (swim_readbit(base, TRACK_ZERO))
398                         break;
399
400                 if (swim_step(base))
401                         return -1;
402         }
403
404         if (swim_readbit(base, TRACK_ZERO))
405                 return 0;
406
407         return -1;
408 }
409
410 static inline int swim_seek(struct swim __iomem *base, int step)
411 {
412         if (step == 0)
413                 return 0;
414
415         if (step < 0) {
416                 swim_action(base, SEEK_NEGATIVE);
417                 step = -step;
418         } else
419                 swim_action(base, SEEK_POSITIVE);
420
421         for ( ; step > 0; step--) {
422                 if (swim_step(base))
423                         return -1;
424         }
425
426         return 0;
427 }
428
429 static inline int swim_track(struct floppy_state *fs,  int track)
430 {
431         struct swim __iomem *base = fs->swd->base;
432         int ret;
433
434         ret = swim_seek(base, track - fs->track);
435
436         if (ret == 0)
437                 fs->track = track;
438         else {
439                 swim_track00(base);
440                 fs->track = 0;
441         }
442
443         return ret;
444 }
445
446 static int floppy_eject(struct floppy_state *fs)
447 {
448         struct swim __iomem *base = fs->swd->base;
449
450         swim_drive(base, fs->location);
451         swim_motor(base, OFF);
452         swim_eject(base);
453
454         fs->disk_in = 0;
455         fs->ejected = 1;
456
457         return 0;
458 }
459
460 static inline int swim_read_sector(struct floppy_state *fs,
461                                    int side, int track,
462                                    int sector, unsigned char *buffer)
463 {
464         struct swim __iomem *base = fs->swd->base;
465         unsigned long flags;
466         struct sector_header header;
467         int ret = -1;
468         short i;
469
470         swim_track(fs, track);
471
472         swim_write(base, mode1, MOTON);
473         swim_head(base, side);
474         swim_write(base, mode0, side);
475
476         local_irq_save(flags);
477         for (i = 0; i < 36; i++) {
478                 ret = swim_read_sector_header(base, &header);
479                 if (!ret && (header.sector == sector)) {
480                         /* found */
481
482                         ret = swim_read_sector_data(base, buffer);
483                         break;
484                 }
485         }
486         local_irq_restore(flags);
487
488         swim_write(base, mode0, MOTON);
489
490         if ((header.side != side)  || (header.track != track) ||
491              (header.sector != sector))
492                 return 0;
493
494         return ret;
495 }
496
497 static int floppy_read_sectors(struct floppy_state *fs,
498                                int req_sector, int sectors_nb,
499                                unsigned char *buffer)
500 {
501         struct swim __iomem *base = fs->swd->base;
502         int ret;
503         int side, track, sector;
504         int i, try;
505
506
507         swim_drive(base, fs->location);
508         for (i = req_sector; i < req_sector + sectors_nb; i++) {
509                 int x;
510                 track = i / fs->secpercyl;
511                 x = i % fs->secpercyl;
512                 side = x / fs->secpertrack;
513                 sector = x % fs->secpertrack + 1;
514
515                 try = 5;
516                 do {
517                         ret = swim_read_sector(fs, side, track, sector,
518                                                 buffer);
519                         if (try-- == 0)
520                                 return -EIO;
521                 } while (ret != 512);
522
523                 buffer += ret;
524         }
525
526         return 0;
527 }
528
529 static void redo_fd_request(struct request_queue *q)
530 {
531         struct request *req;
532         struct floppy_state *fs;
533
534         req = blk_fetch_request(q);
535         while (req) {
536                 int err = -EIO;
537
538                 fs = req->rq_disk->private_data;
539                 if (blk_rq_pos(req) >= fs->total_secs)
540                         goto done;
541                 if (!fs->disk_in)
542                         goto done;
543                 if (rq_data_dir(req) == WRITE && fs->write_protected)
544                         goto done;
545
546                 switch (rq_data_dir(req)) {
547                 case WRITE:
548                         /* NOT IMPLEMENTED */
549                         break;
550                 case READ:
551                         err = floppy_read_sectors(fs, blk_rq_pos(req),
552                                                   blk_rq_cur_sectors(req),
553                                                   req->buffer);
554                         break;
555                 }
556         done:
557                 if (!__blk_end_request_cur(req, err))
558                         req = blk_fetch_request(q);
559         }
560 }
561
562 static void do_fd_request(struct request_queue *q)
563 {
564         redo_fd_request(q);
565 }
566
567 static struct floppy_struct floppy_type[4] = {
568         {    0,  0, 0,  0, 0, 0x00, 0x00, 0x00, 0x00, NULL }, /* no testing   */
569         {  720,  9, 1, 80, 0, 0x2A, 0x02, 0xDF, 0x50, NULL }, /* 360KB SS 3.5"*/
570         { 1440,  9, 2, 80, 0, 0x2A, 0x02, 0xDF, 0x50, NULL }, /* 720KB 3.5"   */
571         { 2880, 18, 2, 80, 0, 0x1B, 0x00, 0xCF, 0x6C, NULL }, /* 1.44MB 3.5"  */
572 };
573
574 static int get_floppy_geometry(struct floppy_state *fs, int type,
575                                struct floppy_struct **g)
576 {
577         if (type >= ARRAY_SIZE(floppy_type))
578                 return -EINVAL;
579
580         if (type)
581                 *g = &floppy_type[type];
582         else if (fs->type == HD_MEDIA) /* High-Density media */
583                 *g = &floppy_type[3];
584         else if (fs->head_number == 2) /* double-sided */
585                 *g = &floppy_type[2];
586         else
587                 *g = &floppy_type[1];
588
589         return 0;
590 }
591
592 static void setup_medium(struct floppy_state *fs)
593 {
594         struct swim __iomem *base = fs->swd->base;
595
596         if (swim_readbit(base, DISK_IN)) {
597                 struct floppy_struct *g;
598                 fs->disk_in = 1;
599                 fs->write_protected = swim_readbit(base, WRITE_PROT);
600                 fs->type = swim_readbit(base, ONEMEG_MEDIA);
601
602                 if (swim_track00(base))
603                         printk(KERN_ERR
604                                 "SWIM: cannot move floppy head to track 0\n");
605
606                 swim_track00(base);
607
608                 get_floppy_geometry(fs, 0, &g);
609                 fs->total_secs = g->size;
610                 fs->secpercyl = g->head * g->sect;
611                 fs->secpertrack = g->sect;
612                 fs->track = 0;
613         } else {
614                 fs->disk_in = 0;
615         }
616 }
617
618 static int floppy_open(struct block_device *bdev, fmode_t mode)
619 {
620         struct floppy_state *fs = bdev->bd_disk->private_data;
621         struct swim __iomem *base = fs->swd->base;
622         int err;
623
624         if (fs->ref_count == -1 || (fs->ref_count && mode & FMODE_EXCL))
625                 return -EBUSY;
626
627         if (mode & FMODE_EXCL)
628                 fs->ref_count = -1;
629         else
630                 fs->ref_count++;
631
632         swim_write(base, setup, S_IBM_DRIVE  | S_FCLK_DIV2);
633         udelay(10);
634         swim_drive(base, INTERNAL_DRIVE);
635         swim_motor(base, ON);
636         swim_action(base, SETMFM);
637         if (fs->ejected)
638                 setup_medium(fs);
639         if (!fs->disk_in) {
640                 err = -ENXIO;
641                 goto out;
642         }
643
644         if (mode & FMODE_NDELAY)
645                 return 0;
646
647         if (mode & (FMODE_READ|FMODE_WRITE)) {
648                 check_disk_change(bdev);
649                 if ((mode & FMODE_WRITE) && fs->write_protected) {
650                         err = -EROFS;
651                         goto out;
652                 }
653         }
654         return 0;
655 out:
656         if (fs->ref_count < 0)
657                 fs->ref_count = 0;
658         else if (fs->ref_count > 0)
659                 --fs->ref_count;
660
661         if (fs->ref_count == 0)
662                 swim_motor(base, OFF);
663         return err;
664 }
665
666 static int floppy_unlocked_open(struct block_device *bdev, fmode_t mode)
667 {
668         int ret;
669
670         mutex_lock(&swim_mutex);
671         ret = floppy_open(bdev, mode);
672         mutex_unlock(&swim_mutex);
673
674         return ret;
675 }
676
677 static int floppy_release(struct gendisk *disk, fmode_t mode)
678 {
679         struct floppy_state *fs = disk->private_data;
680         struct swim __iomem *base = fs->swd->base;
681
682         mutex_lock(&swim_mutex);
683         if (fs->ref_count < 0)
684                 fs->ref_count = 0;
685         else if (fs->ref_count > 0)
686                 --fs->ref_count;
687
688         if (fs->ref_count == 0)
689                 swim_motor(base, OFF);
690         mutex_unlock(&swim_mutex);
691
692         return 0;
693 }
694
695 static int floppy_ioctl(struct block_device *bdev, fmode_t mode,
696                         unsigned int cmd, unsigned long param)
697 {
698         struct floppy_state *fs = bdev->bd_disk->private_data;
699         int err;
700
701         if ((cmd & 0x80) && !capable(CAP_SYS_ADMIN))
702                         return -EPERM;
703
704         switch (cmd) {
705         case FDEJECT:
706                 if (fs->ref_count != 1)
707                         return -EBUSY;
708                 mutex_lock(&swim_mutex);
709                 err = floppy_eject(fs);
710                 mutex_unlock(&swim_mutex);
711                 return err;
712
713         case FDGETPRM:
714                 if (copy_to_user((void __user *) param, (void *) &floppy_type,
715                                  sizeof(struct floppy_struct)))
716                         return -EFAULT;
717                 break;
718
719         default:
720                 printk(KERN_DEBUG "SWIM floppy_ioctl: unknown cmd %d\n",
721                        cmd);
722                 return -ENOSYS;
723         }
724         return 0;
725 }
726
727 static int floppy_getgeo(struct block_device *bdev, struct hd_geometry *geo)
728 {
729         struct floppy_state *fs = bdev->bd_disk->private_data;
730         struct floppy_struct *g;
731         int ret;
732
733         ret = get_floppy_geometry(fs, 0, &g);
734         if (ret)
735                 return ret;
736
737         geo->heads = g->head;
738         geo->sectors = g->sect;
739         geo->cylinders = g->track;
740
741         return 0;
742 }
743
744 static unsigned int floppy_check_events(struct gendisk *disk,
745                                         unsigned int clearing)
746 {
747         struct floppy_state *fs = disk->private_data;
748
749         return fs->ejected ? DISK_EVENT_MEDIA_CHANGE : 0;
750 }
751
752 static int floppy_revalidate(struct gendisk *disk)
753 {
754         struct floppy_state *fs = disk->private_data;
755         struct swim __iomem *base = fs->swd->base;
756
757         swim_drive(base, fs->location);
758
759         if (fs->ejected)
760                 setup_medium(fs);
761
762         if (!fs->disk_in)
763                 swim_motor(base, OFF);
764         else
765                 fs->ejected = 0;
766
767         return !fs->disk_in;
768 }
769
770 static const struct block_device_operations floppy_fops = {
771         .owner           = THIS_MODULE,
772         .open            = floppy_unlocked_open,
773         .release         = floppy_release,
774         .ioctl           = floppy_ioctl,
775         .getgeo          = floppy_getgeo,
776         .check_events    = floppy_check_events,
777         .revalidate_disk = floppy_revalidate,
778 };
779
780 static struct kobject *floppy_find(dev_t dev, int *part, void *data)
781 {
782         struct swim_priv *swd = data;
783         int drive = (*part & 3);
784
785         if (drive > swd->floppy_count)
786                 return NULL;
787
788         *part = 0;
789         return get_disk(swd->unit[drive].disk);
790 }
791
792 static int __devinit swim_add_floppy(struct swim_priv *swd,
793                                      enum drive_location location)
794 {
795         struct floppy_state *fs = &swd->unit[swd->floppy_count];
796         struct swim __iomem *base = swd->base;
797
798         fs->location = location;
799
800         swim_drive(base, location);
801
802         swim_motor(base, OFF);
803
804         if (swim_readbit(base, SINGLE_SIDED))
805                 fs->head_number = 1;
806         else
807                 fs->head_number = 2;
808         fs->ref_count = 0;
809         fs->ejected = 1;
810
811         swd->floppy_count++;
812
813         return 0;
814 }
815
816 static int __devinit swim_floppy_init(struct swim_priv *swd)
817 {
818         int err;
819         int drive;
820         struct swim __iomem *base = swd->base;
821
822         /* scan floppy drives */
823
824         swim_drive(base, INTERNAL_DRIVE);
825         if (swim_readbit(base, DRIVE_PRESENT))
826                 swim_add_floppy(swd, INTERNAL_DRIVE);
827         swim_drive(base, EXTERNAL_DRIVE);
828         if (swim_readbit(base, DRIVE_PRESENT))
829                 swim_add_floppy(swd, EXTERNAL_DRIVE);
830
831         /* register floppy drives */
832
833         err = register_blkdev(FLOPPY_MAJOR, "fd");
834         if (err) {
835                 printk(KERN_ERR "Unable to get major %d for SWIM floppy\n",
836                        FLOPPY_MAJOR);
837                 return -EBUSY;
838         }
839
840         for (drive = 0; drive < swd->floppy_count; drive++) {
841                 swd->unit[drive].disk = alloc_disk(1);
842                 if (swd->unit[drive].disk == NULL) {
843                         err = -ENOMEM;
844                         goto exit_put_disks;
845                 }
846                 swd->unit[drive].swd = swd;
847         }
848
849         swd->queue = blk_init_queue(do_fd_request, &swd->lock);
850         if (!swd->queue) {
851                 err = -ENOMEM;
852                 goto exit_put_disks;
853         }
854
855         for (drive = 0; drive < swd->floppy_count; drive++) {
856                 swd->unit[drive].disk->flags = GENHD_FL_REMOVABLE;
857                 swd->unit[drive].disk->major = FLOPPY_MAJOR;
858                 swd->unit[drive].disk->first_minor = drive;
859                 sprintf(swd->unit[drive].disk->disk_name, "fd%d", drive);
860                 swd->unit[drive].disk->fops = &floppy_fops;
861                 swd->unit[drive].disk->private_data = &swd->unit[drive];
862                 swd->unit[drive].disk->queue = swd->queue;
863                 set_capacity(swd->unit[drive].disk, 2880);
864                 add_disk(swd->unit[drive].disk);
865         }
866
867         blk_register_region(MKDEV(FLOPPY_MAJOR, 0), 256, THIS_MODULE,
868                             floppy_find, NULL, swd);
869
870         return 0;
871
872 exit_put_disks:
873         unregister_blkdev(FLOPPY_MAJOR, "fd");
874         while (drive--)
875                 put_disk(swd->unit[drive].disk);
876         return err;
877 }
878
879 static int __devinit swim_probe(struct platform_device *dev)
880 {
881         struct resource *res;
882         struct swim __iomem *swim_base;
883         struct swim_priv *swd;
884         int ret;
885
886         res = platform_get_resource(dev, IORESOURCE_MEM, 0);
887         if (!res) {
888                 ret = -ENODEV;
889                 goto out;
890         }
891
892         if (!request_mem_region(res->start, resource_size(res), CARDNAME)) {
893                 ret = -EBUSY;
894                 goto out;
895         }
896
897         swim_base = ioremap(res->start, resource_size(res));
898         if (!swim_base) {
899                 return -ENOMEM;
900                 goto out_release_io;
901         }
902
903         /* probe device */
904
905         set_swim_mode(swim_base, 1);
906         if (!get_swim_mode(swim_base)) {
907                 printk(KERN_INFO "SWIM device not found !\n");
908                 ret = -ENODEV;
909                 goto out_iounmap;
910         }
911
912         /* set platform driver data */
913
914         swd = kzalloc(sizeof(struct swim_priv), GFP_KERNEL);
915         if (!swd) {
916                 ret = -ENOMEM;
917                 goto out_iounmap;
918         }
919         platform_set_drvdata(dev, swd);
920
921         swd->base = swim_base;
922
923         ret = swim_floppy_init(swd);
924         if (ret)
925                 goto out_kfree;
926
927         return 0;
928
929 out_kfree:
930         platform_set_drvdata(dev, NULL);
931         kfree(swd);
932 out_iounmap:
933         iounmap(swim_base);
934 out_release_io:
935         release_mem_region(res->start, resource_size(res));
936 out:
937         return ret;
938 }
939
940 static int __devexit swim_remove(struct platform_device *dev)
941 {
942         struct swim_priv *swd = platform_get_drvdata(dev);
943         int drive;
944         struct resource *res;
945
946         blk_unregister_region(MKDEV(FLOPPY_MAJOR, 0), 256);
947
948         for (drive = 0; drive < swd->floppy_count; drive++) {
949                 del_gendisk(swd->unit[drive].disk);
950                 put_disk(swd->unit[drive].disk);
951         }
952
953         unregister_blkdev(FLOPPY_MAJOR, "fd");
954
955         blk_cleanup_queue(swd->queue);
956
957         /* eject floppies */
958
959         for (drive = 0; drive < swd->floppy_count; drive++)
960                 floppy_eject(&swd->unit[drive]);
961
962         iounmap(swd->base);
963
964         res = platform_get_resource(dev, IORESOURCE_MEM, 0);
965         if (res)
966                 release_mem_region(res->start, resource_size(res));
967
968         platform_set_drvdata(dev, NULL);
969         kfree(swd);
970
971         return 0;
972 }
973
974 static struct platform_driver swim_driver = {
975         .probe  = swim_probe,
976         .remove = __devexit_p(swim_remove),
977         .driver   = {
978                 .name   = CARDNAME,
979                 .owner  = THIS_MODULE,
980         },
981 };
982
983 static int __init swim_init(void)
984 {
985         printk(KERN_INFO "SWIM floppy driver %s\n", DRIVER_VERSION);
986
987         return platform_driver_register(&swim_driver);
988 }
989 module_init(swim_init);
990
991 static void __exit swim_exit(void)
992 {
993         platform_driver_unregister(&swim_driver);
994 }
995 module_exit(swim_exit);
996
997 MODULE_DESCRIPTION("Driver for SWIM floppy controller");
998 MODULE_LICENSE("GPL");
999 MODULE_AUTHOR("Laurent Vivier <laurent@lvivier.info>");
1000 MODULE_ALIAS_BLOCKDEV_MAJOR(FLOPPY_MAJOR);