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